diff --git a/.gitignore b/.gitignore index 033c19e..5f880a7 100644 --- a/.gitignore +++ b/.gitignore @@ -48,3 +48,166 @@ $RECYCLE.BIN/ Network Trash Folder Temporary Items .apdisk + +docs/_build/ + +.vscode/* + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ diff --git a/demos/cart_pole/bo_loss_func.m b/demos/cart_pole/bo_loss_func.m new file mode 100644 index 0000000..85df3b6 --- /dev/null +++ b/demos/cart_pole/bo_loss_func.m @@ -0,0 +1,18 @@ +function loss = bo_loss_func(y, x) + loss1 = -y^2; + loss2 = -(x(3)-pi)^2; + loss3 = -x(4)^2; + loss4 = -x(2)^2; + % fprintf("Loss: y_error: %.4f \t theta: %.4f \t dtheta: %.4f \t dx: %.4f \n", [loss1, loss2, loss3, loss4]); + % c1 = 1000; + c1 = 0; + % c2 = 1000; + % c2 = 200; + c2 = 100; + c3 = 1; + % c4 = 100; + c4 = 0; + fprintf("Loss: y_error: %.4f \t theta: %.4f \t dtheta: %.4f \t dx: %.4f \n", [c1 * loss1, c2 * loss2, c3 * loss3, c4 * loss4]); + loss = c1 * loss1 + c2 * loss2 + c3 * loss3 + c4 * loss4; + +end \ No newline at end of file diff --git a/demos/cart_pole/clip_theta.m b/demos/cart_pole/clip_theta.m new file mode 100644 index 0000000..688e49d --- /dev/null +++ b/demos/cart_pole/clip_theta.m @@ -0,0 +1,10 @@ +function x_clipped = clip_theta(x) + x_clipped = x; + theta = x(3); + r = mod(theta, 2*pi); + if r > pi + x_clipped(3) = r - 2*pi; + else + x_clipped(3) = r; + end +end \ No newline at end of file diff --git a/demos/cart_pole/init_settings_cart_pole.m b/demos/cart_pole/init_settings_cart_pole.m new file mode 100644 index 0000000..5f43e76 --- /dev/null +++ b/demos/cart_pole/init_settings_cart_pole.m @@ -0,0 +1,52 @@ +clear all; +close all; +dt = 0.01; + +%% Direct CLF without FL works for these parameters. +% params.l = 1.0; % [m] length of pendulum +% params.m = 1.0; % [kg] mass of pendulum +% params.M = 1.0; % [kg] mass of cart +% params.g = 9.81; % [m/s^2] acceleration of gravity +% params.b = 0.1; % [s*Nm/rad] friction coefficient + +params.l = 0.5; % [m] length of pendulum +params.m = 0.5; % [kg] mass of pendulum +params.M = 2.0; % [kg] mass of cart +params.g = 9.81; % [m/s^2] acceleration of gravity +params.b = 0.1; % [s*Nm/rad] friction coefficient + +params.cbf.rate = 1; + +u_bound = 50; +params.u_max = u_bound; +params.u_min = -u_bound; + +params.ratio_u_diff = 0; +params.weight.slack = 1000; +%params.weight.input = eye(params.udim); % tune this to allocate different +%weight in each u coordinate +params.clf.rate = 1; + +%% Feedback linearization specific params. +params.bezier = [-0.0630040307234950, ... + -2.94395202983077, ... + 4.23655180750478, ... + -10.8954633332227, ... + 13.3619387476085, ... + -13.4630778220299, ... + 9.93449615595792, ... + -3.07150993759866, ... + 3.86268687914481, ... + 2.72221821558065, ... + 3.15006153603993]; +params.phase_max = 1; +params.phase_min = 0; +params.epsilon_FL = 0.015; +params.Kp_FL = 0.99; +params.Kd_FL = 2.0; + +control_sys = CartPole(params); + +plant_sys = CartPole(params); + +settings.dt = dt; diff --git a/demos/cart_pole/main_cart_pole.m b/demos/cart_pole/main_cart_pole.m new file mode 100644 index 0000000..7a50de1 --- /dev/null +++ b/demos/cart_pole/main_cart_pole.m @@ -0,0 +1,21 @@ +init_settings_cart_pole; +verbose = 0; + +with_slack = 1; +% initial state +% x0 = [0; 0; pi - 0.1; 0;]; +x0 = [0; 0; 0.1; 0;]; +% simulation time +sim_t = 3; + + +[xs, us, ts, Vs, ~, ~, ~, ~] = rollout_lyapunov_controller( ... + x0, plant_sys, control_sys, @control_sys.ctrlClfQp, dt, sim_t, with_slack, [], verbose); +results.vanilla.trajectory = xs; +results.vanilla.controls = us; +results.vanilla.stamps = ts; + +draw_rollout(results.vanilla, 0, dt, plant_sys, control_sys, "Vanilla") + +figure; +plot(ts, Vs); \ No newline at end of file diff --git a/demos/cart_pole/main_cart_pole_BO.m b/demos/cart_pole/main_cart_pole_BO.m new file mode 100644 index 0000000..5a5da2b --- /dev/null +++ b/demos/cart_pole/main_cart_pole_BO.m @@ -0,0 +1,137 @@ +init_settings_cart_pole; +verbose = 0; + +with_slack = 1; +% initial state +% x0 = [0; 0; pi-0.1; 0;]; +x0 = [0; 0; 0; 0;]; +% simulation time +sim_t = 2.0; + +%% PHASE 1 + +tune_params_grid = cell(4, 1); +% tune_params_grid{1} = 1:1:100; % Kp +% tune_params_grid{2} = 1:1:100; % Kd +% tune_params_grid{3} = 0.005:0.005:0.2; % eps +tune_params_grid{1} = [1, 2, 4, 8, 10, 16, 20, 25, 30, 32, 35, 40, 50, 60, 64]; % Kp +tune_params_grid{2} = [0.5, 1, 1.5, 2, 4, 8, 10, 16, 18, 20, 25, 32, 40]; % Kd +tune_params_grid{3} = 0.01:0.01:0.2; % eps +tune_params_grid{4} = 1:1:10; % clf rate + +% tune_params_grid{1} = 24:0.2:35; % Kp +% tune_params_grid{2} = 16:1:42; % Kd +% tune_params_grid{3} = 0.01:0.005:0.2; % eps +% tune_params_grid{4} = 1:1:10; % clf rate + +tune_meshgrids = cell(4, 1); +[tune_meshgrids{:}] = ndgrid(tune_params_grid{:}); +tune_params_grid = []; +for i = 1:4 + tune_params_grid = [tune_params_grid, tune_meshgrids{i}(:)]; +end + +hyp = struct('mean', [], 'cov', [log(10.0); log(1.2)], 'lik', log(1e-2)); +likfunc = @likGauss; +covfunc = @covSEiso; +lik = log(0.1); +gp_handle = GPHandleKernelBaseline(4, covfunc, hyp, likfunc, lik); + +eval_params = [28, 25, 0.15, 1]; +eval_loss = []; +num_iter = 80; +beta = 1.0; +tune_params_grid_original = tune_params_grid; + +for k = 1:num_iter + disp(k); + params.Kp_FL = eval_params(end, 1); + params.Kd_FL = eval_params(end, 2); + params.epsilon_FL = eval_params(end, 3); + params.clf.rate = eval_params(end, 4); + control_sys = CartPole(params); + [xs, us, ts, Vs, ~, ys] = rollout_feedback_linearization( ... + x0, plant_sys, control_sys, @control_sys.ctrlClfQpFL, dt, sim_t, with_slack, [], verbose); + loss_out = bo_loss_func(ys(end), xs(end, :)); + eval_loss = [eval_loss; loss_out]; + + gp_handle.update_training_data(eval_params, eval_loss); + [zs_mu, zs_s2] = gp_handle.get_posterior(tune_params_grid); + acquisition_vals = zs_mu + beta * sqrt(zs_s2); + [~, idx] = max(acquisition_vals); + new_eval_params = tune_params_grid(idx, :); + tune_params_grid = tune_params_grid([1:idx-1,idx+1:end], :); + eval_params = [eval_params; new_eval_params]; +end + +[zs_mu, zs_s2] = gp_handle.get_posterior(tune_params_grid_original); +[~, max_indices] = maxk(zs_mu, 10); +disp("Phase 1: Best tuning parameters:"); +max_tune_params = tune_params_grid_original(max_indices, :); +disp(max_tune_params); + +[hyp_trained, lik_trained] = gp_handle.get_optimized_hyperparameters(hyp, lik); +disp("New hyperparams suggeestions") +disp(hyp_trained.cov) +disp(lik_trained) + +disp("PHASE 2 initiating."); + +%% PHASE 2 + +new_grid_max = max(max_tune_params); +new_grid_min = min(max_tune_params); +for i = 1:4 + if (new_grid_min(i) == new_grid_max(i)) + new_grid_min(i) = new_grid_min(i) / 2; + new_grid_max(i) = new_grid_min(i) * 3; + end +end + +tune_params_grid = cell(4, 1); +tune_params_grid{1} = new_grid_min(1):0.5:new_grid_max(1); % Kp +tune_params_grid{2} = new_grid_min(2):0.5:new_grid_max(2); % Kd +tune_params_grid{3} = new_grid_min(3):0.005:new_grid_max(3); % eps +tune_params_grid{4} = new_grid_min(4):0.1:new_grid_max(4); % eps + +tune_meshgrids = cell(4, 1); +[tune_meshgrids{:}] = ndgrid(tune_params_grid{:}); +tune_params_grid = []; +for i = 1:4 + tune_params_grid = [tune_params_grid, tune_meshgrids{i}(:)]; +end + +gp_handle = GPHandleKernelBaseline(4, covfunc, hyp_trained, likfunc, lik_trained); + +eval_params = eval_params(end, :); +eval_loss = []; +num_iter = 20; +beta = 1.0; +tune_params_grid_original = tune_params_grid; + +for k = 1:num_iter + disp(k); + params.Kp_FL = eval_params(end, 1); + params.Kd_FL = eval_params(end, 2); + params.epsilon_FL = eval_params(end, 3); + params.clf.rate = eval_params(end, 4); + control_sys = CartPole(params); + [xs, us, ts, Vs, ~, ys] = rollout_feedback_linearization( ... + x0, plant_sys, control_sys, @control_sys.ctrlClfQpFL, dt, sim_t, with_slack, [], verbose); + loss_out = bo_loss_func(ys(end), xs(end, :)); + eval_loss = [eval_loss; loss_out]; + + gp_handle.update_training_data(eval_params, eval_loss); + [zs_mu, zs_s2] = gp_handle.get_posterior(tune_params_grid); + acquisition_vals = zs_mu + beta * sqrt(zs_s2); + [~, idx] = max(acquisition_vals); + new_eval_params = tune_params_grid(idx, :); + tune_params_grid = tune_params_grid([1:idx-1,idx+1:end], :); + eval_params = [eval_params; new_eval_params]; +end + +[zs_mu, zs_s2] = gp_handle.get_posterior(tune_params_grid_original); +[~, max_indices] = maxk(zs_mu, 10); +disp("Phase 2: Best tuning parameters:"); +max_tune_params = tune_params_grid_original(max_indices, :); +disp(max_tune_params); \ No newline at end of file diff --git a/demos/cart_pole/main_cart_pole_FL.m b/demos/cart_pole/main_cart_pole_FL.m new file mode 100644 index 0000000..41b0cb9 --- /dev/null +++ b/demos/cart_pole/main_cart_pole_FL.m @@ -0,0 +1,62 @@ +init_settings_cart_pole; +verbose = 1; + +with_slack = 1; +% initial state +% x0 = [0; 0; pi-0.1; 0;]; +x0 = [0; 0; 0; 0;]; +% simulation time +sim_t = 1.65; + +[xs, us, ts, Vs, ~, ys] = rollout_feedback_linearization( ... + x0, plant_sys, control_sys, @control_sys.ctrlClfQpFL, dt, sim_t, with_slack, [], verbose); +results.vanilla.trajectory = xs; +results.vanilla.controls = us; +results.vanilla.stamps = ts; + +phase = xs(:, 1) + 0.5*xs(:, 3); +c_normal = 2.37079632679490; +phase = phase / c_normal; +y_d = control_sys.bezier(control_sys.params.bezier, phase); + + +figure +subplot(4, 2, 1); +plot(ts, xs(:, 1)); +ylabel('x'); + +subplot(4, 2, 2); +plot(ts, xs(:, 3)); hold on; +plot(ts, y_d); +ylabel('theta'); +legend('theta', 'y_d'); + +subplot(4, 2, 3); +plot(ts, xs(:, 2)); +ylabel('dx'); + +subplot(4, 2, 4); +plot(ts, xs(:, 4)); +ylabel('dtheta'); + +subplot(4, 2, 5); +plot(ts(1:end-1), us); +ylabel('u'); + +subplot(4, 2, 6); +plot(ts, Vs); +ylabel('V'); + +subplot(4, 2, 7); +plot(ts, phase); +ylabel('phase'); + +subplot(4, 2, 8); +plot(ts, ys); +ylabel('y_error'); + +% draw_rollout(results.vanilla, 0, dt, plant_sys, control_sys, "Vanilla") + +function mu = dummy_controller(x, ~, ~) + mu = 0; +end \ No newline at end of file diff --git a/demos/cart_pole/vis/draw_cart_pole.m b/demos/cart_pole/vis/draw_cart_pole.m new file mode 100644 index 0000000..f5cde46 --- /dev/null +++ b/demos/cart_pole/vis/draw_cart_pole.m @@ -0,0 +1,81 @@ +%% draw_cp.m +% *Summary:* Draw the cart-pole system with reward, applied force, and +% predictive uncertainty of the tip of the pendulum +% +% function draw_cp(x, theta, force, cost, text1, text2, M, S) +% +% +% *Input arguments:* +% +% x position of the cart +% theta angle of pendulum +% force force applied to cart +% cost cost structure +% .fcn function handle (it is assumed to use saturating cost) +% .<> other fields that are passed to cost +% M (optional) mean of state +% S (optional) covariance of state +% text1 (optional) text field 1 +% text2 (optional) text field 2 +% +% +% Copyright (C) 2008-2013 by +% Marc Deisenroth, Andrew McHutchon, Joe Hall, and Carl Edward Rasmussen. +% +% Last modified: 2013-03-07 + +function draw_cart_pole(x, theta, force, text1, text2, plant_sys, control_sys) +% function draw_cp(x, theta, force, cost, text1, text2, M, S) +%% Code + +l = plant_sys.params.l; +xmin = -10; +xmax = 10; +height = 0.1; +width = 0.3; +maxU = control_sys.params.u_max; + +% Compute positions +cart = [ x + width, height + x + width, -height + x - width, -height + x - width, height + x + width, height ]; +pendulum = [x, 0; x+2*l*sin(theta), -cos(theta)*2*l]; + + +clf; hold on +plot(0,2*l,'k+','MarkerSize',20,'linewidth',2) +plot([xmin, xmax], [-height-0.03, -height-0.03],'k','linewidth',2) + +% Plot force +plot([0 force/maxU*xmax],[-0.3, -0.3],'g','linewidth',10) + +% Plot the cart-pole +fill(cart(:,1), cart(:,2),'k','edgecolor','k'); +plot(pendulum(:,1), pendulum(:,2),'r','linewidth',4) + +% Plot the joint and the tip +plot(x,0,'y.','markersize',24) +plot(pendulum(2,1),pendulum(2,2),'y.','markersize',24) + +% plot ellipse around tip of pendulum (if M, S exist) +try + [M1 S1] = getPlotDistr_cp(M,S,2*l); + error_ellipse(S1,M1,'style','b'); +catch +end + +% Text +text(0,-0.3,'applied force') +% text(0,-0.5,'immediate reward') +if exist('text1','var') + text(0,-0.9, text1) +end +if exist('text2','var') + text(0,-1.1, text2) +end + +set(gca,'DataAspectRatio',[1 1 1],'XLim',[xmin xmax],'YLim',[-2 2]); +axis off; +drawnow; diff --git a/demos/cart_pole/vis/draw_rollout.m b/demos/cart_pole/vis/draw_rollout.m new file mode 100644 index 0000000..767daa7 --- /dev/null +++ b/demos/cart_pole/vis/draw_rollout.m @@ -0,0 +1,9 @@ +function draw_rollout(result, j, dt, plant_sys, control_sys, title_text) +for r = 1:size(result.trajectory,1)-1 + draw_cart_pole(result.trajectory(r,1), result.trajectory(r,3), result.controls(r), ... + ['trial # ' num2str(j) ', t=' num2str(r*dt) ' sec'], ... + [title_text], plant_sys, control_sys); + pause(dt); +end + +end \ No newline at end of file diff --git a/demos/rabbit/anim/anim_flat_ground.m b/demos/rabbit/anim/anim_flat_ground.m new file mode 100644 index 0000000..6e4670f --- /dev/null +++ b/demos/rabbit/anim/anim_flat_ground.m @@ -0,0 +1,168 @@ +function anim_flat_ground(t,x,ts) +%ANIM Animate kneed biped walker. Note: (1) TS is an options +% parameters and (2) T may be a scalar. + +%Eric Westervelt +%2/15/01 + +if nargin < 3, ts = 1/20; end + +[n,m] = size(x); +pH_horiz = zeros(n,1); + +if n == 1 | m == 1 + te = t; + xe = x; + q=xe(1:5); + n = 1; +else + % Estimate hip horizontal position by estimating integral of hip + % velocity + vH = hip_vel(x); % convert angles to horizontal hosition of hips + for j=2:n + pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); + end + + [te,pH_horiz]=even_sample(t,pH_horiz,1/ts); + [te,xe]=even_sample(t,x,1/ts); + [n,m]=size(xe); + q=xe(1,1:5); +end + +k=0; + +out = limb_position(q,pH_horiz(1)); + +fig_hl=figure(1); +%set(fig_hl,'Position', [440 700 250 250]); % extra small +%set(fig_hl,'Position', [440 500 250 250]); % for laptop +set(fig_hl,'Position', [20 80 250 250]); % for serefe +%set(fig_hl,'Position', [550 660 400 400]); % for making movies +set(fig_hl,'PaperPosition',[0 0 5 5]); % for making movies +clf +%anim_axis=[-.8 .8 -1.5 1.4]; % for falling animation +anim_axis=[-.8 .8 -0.1 1.4]; +axis off +axis(anim_axis) +axis equal +grid + +% Use actual relations between masses in animation + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; +scl=0.022; % factor to scale masses +mr_knees=M4^(1/3)*scl; % radius of mass for knees +mr_femurs=M3^(1/3)*scl; % radius of mass for femurs +mr_torso=M1^(1/3)*scl; % radius of mass for torso +leg1_color='g'; +leg2_color='r'; +torso_color='b'; +ground_color='k'; % a.k.a. black + +% Approximate circular shape of mass +param=linspace(0,2*pi+2*pi/50,50); +xmass_knees=mr_knees*cos(param); +ymass_knees=mr_knees*sin(param); + +xmass_femurs=mr_femurs*cos(param); +ymass_femurs=mr_femurs*sin(param); + +xmass_torso=mr_torso*cos(param); +ymass_torso=mr_torso*sin(param); + +% Draw ground +buffer=5; +ground=line([-buffer pH_horiz(n)+buffer],[0 0]); +set(ground,'Color',ground_color,'LineWidth',2); +for k=-buffer:floor(pH_horiz(n)+buffer) + ref_tick(k+buffer+1)=line([k k],[-0.07 0]); + set(ref_tick(k+buffer+1),'Color',ground_color); + ref_label(k+buffer+1)=text(-0.03+k,-0.18,num2str(k)); + set(ref_label(k+buffer+1),'FontSize',15) +end + +% Draw stance leg +tibia1=line([out.pFoot11 out.pG11],[out.pFoot12 out.pG12]); +tibia_mass1=patch(xmass_knees+out.pTib11,ymass_knees+out.pTib12, ... + leg1_color); +femur1=line([out.pG11 out.pH1],[out.pG12 out.pH2]); +femur_mass1=patch(xmass_femurs+out.pFem11,ymass_femurs+out.pFem12, ... + leg1_color); +set(tibia_mass1,'EdgeColor',leg1_color) +set(tibia1,'LineWidth',2,'Color',leg1_color); +set(femur_mass1,'EdgeColor',leg1_color) +set(femur1,'LineWidth',2,'Color',leg1_color); + +% Draw swing leg +tibia2=line([out.pFoot21 out.pG21],[out.pFoot22 out.pG22]); +tibia_mass2=patch(xmass_knees+out.pTib21,ymass_knees+out.pTib22, ... + leg2_color); +femur2=line([out.pG21 out.pH1],[out.pG22 out.pH2]); +femur_mass2=patch(xmass_femurs+out.pFem21,ymass_femurs+out.pFem22, ... + leg2_color); +set(tibia_mass2,'EdgeColor',leg2_color) +set(tibia2,'LineWidth',2,'Color',leg2_color); +set(femur_mass2,'EdgeColor',leg2_color) +set(femur2,'LineWidth',2,'Color',leg2_color); + +% Draw torso +torso=line([out.pH1 out.pHead1],[out.pH2 out.pHead2]); +torso_mass=patch(xmass_torso+out.pT1,ymass_torso+out.pT2, ... + torso_color); +set(torso_mass,'EdgeColor',torso_color) +set(torso,'LineWidth',2,'Color',torso_color); + +for k = 1:n + if n == 1 | m == 1, q=xe(1:5); else, q=xe(k,1:5); end + + out = limb_position(q,pH_horiz(k)); + + % stance leg + set(tibia1,'XData',[out.pFoot11 out.pG11],... + 'YData',[out.pFoot12 out.pG12]); + set(tibia_mass1,'XData',xmass_knees+out.pTib11,... + 'YData',ymass_knees+out.pTib12); + set(femur1,'XData',[out.pG11 out.pH1],... + 'YData',[out.pG12 out.pH2]); + set(femur_mass1,'XData',xmass_femurs+out.pFem11,... + 'YData',ymass_femurs+out.pFem12); + + % swing leg + set(tibia2,'XData',[out.pFoot21 out.pG21],... + 'YData',[out.pFoot22 out.pG22]); + set(tibia_mass2,'XData',xmass_knees+out.pTib21,... + 'YData',ymass_knees+out.pTib22); + set(femur2,'XData',[out.pG21 out.pH1],... + 'YData',[out.pG22 out.pH2]); + set(femur_mass2,'XData',xmass_femurs+out.pFem21,... + 'YData',ymass_femurs+out.pFem22); + + % torso + set(torso,'XData',[out.pH1 out.pHead1],... + 'YData',[out.pH2 out.pHead2]); + set(torso_mass,'XData',xmass_torso+out.pT1,... + 'YData',ymass_torso+out.pT2); + + if n ~= 1 & m ~= 1 + hdl=title(['T_{est} = ',num2str(te(k),'%.2f')]); + set(hdl,'FontSize',15); + end + + new_axis=anim_axis+[out.pH1 out.pH1 0 0]; + axis(new_axis); + + for j = 1:length(ref_label) + if (j-buffer-1new_axis(2)) + set(ref_label(j),'Visible','off') + set(ref_tick(j),'Visible','off') + else + set(ref_label(j),'Visible','on'); + set(ref_tick(j),'Visible','on') + end + end + + drawnow; + + % for making movies + %print('-dppmraw',['frame.',sprintf('%d',k-1),'.ppm']) +end \ No newline at end of file diff --git a/demos/rabbit/anim/anim_moving_stone_load.m b/demos/rabbit/anim/anim_moving_stone_load.m new file mode 100644 index 0000000..5295fc8 --- /dev/null +++ b/demos/rabbit/anim/anim_moving_stone_load.m @@ -0,0 +1,233 @@ +function anim_moving_stone_load(t,x,ts,l_min_t,l_max_t) + +if nargin < 3, ts = 1/20; end + +[n,m] = size(x); +n_full=n; +l_min=l_min_t(1:n); +l_max=l_max_t(1:n); + +% Estimate hip horizontal position by estimating integral of hip +% velocity +pH_horiz = zeros(n,1); +vH = hip_vel(x); % convert angles to horizontal hosition of hips +for j=2:n +pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); +end +pH_horiz_full=pH_horiz; +[te,l_min]=even_sample(t,l_min,1/ts); +[te,l_max]=even_sample(t,l_max,1/ts); +[te,pH_horiz]=even_sample(t,pH_horiz,1/ts); +[te,xe]=even_sample(t,x,1/ts); +[n,m]=size(xe); + + +%save as a video +% spwriter = VideoWriter('biped_walking.avi'); +% set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +% open(spwriter); + +fig1 = figure(1); +set(0,'Units','pixels') +scnsize = get(0,'ScreenSize'); + +screen_width = scnsize(3); +screen_height = scnsize(4); + +figure_x_limits = [-1.5 4];%[-1.5 4]; +figure_y_limits = [-1 2.3];% +% find the minimum scaling factor + +figure_x_size = figure_x_limits(2) - figure_x_limits(1); +figure_y_size = figure_y_limits(2) - figure_y_limits(1); + +xfactor = screen_width/figure_x_size; +yfactor = screen_height/figure_y_size; + +if (xfactor < yfactor) + screen_factor = 0.5*xfactor; +else + screen_factor = 0.5*yfactor; +end + +% calculate screen offsets +screen_x_offset = (screen_width - screen_factor*figure_x_size)/2; +screen_y_offset = (screen_height - screen_factor*figure_y_size)/2; + +% draw figure and axes +set(fig1,'Position', [screen_x_offset screen_y_offset screen_factor*figure_x_size screen_factor*figure_y_size]); +set(fig1,'MenuBar', 'none'); +axes1 = axes; +set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits); +set(axes1,'Position',[0 0 1 1]); +set(axes1,'Color','w'); +set(axes1,'TickDir','out'); + +for k = 1:n + + drawone(axes1, xe, pH_horiz, l_min, l_max, k); + drawnow; + hold on; + pause(ts); + %pause(0.5); +% frame = getframe(gcf); +% writeVideo(spwriter, frame); +end +end + +function drawone(parent, xe, pH_horiz, l_min, l_max, k) +% delete previous screen +tem = get(parent,'Children'); +delete(tem); + +leg1_color='g'; +leg2_color='r'; +torso_color='b'; + +[n,m]=size(xe); + +out = limb_position(xe(k,1:5),pH_horiz(k)); + +%draw stones + draw_ground(xe, pH_horiz, l_min, l_max, k); + +% stance leg + % tibia1: Foot 1 to G1 + a=[out.pFoot11;out.pFoot12];b=[out.pG11;out.pG12]; + pt_link2(a,b,leg1_color,leg1_color); + + % femur 1: H to G1 + a=[out.pG11;out.pG12];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg1_color,leg1_color); + +% swing leg + % tibia2: Foot 2 to G2 + a=[out.pFoot21;out.pFoot22];b=[out.pG21;out.pG22]; + pt_link2(a,b,leg2_color,leg2_color); + + % femur 2: H to G2 + a=[out.pG21;out.pG22];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg2_color,leg2_color); + + +% torso: Head to H + a=[out.pH1;out.pH2];b=[out.pHead1;out.pHead2]; + pt_link2(a,b,torso_color,torso_color); + + +% joints + radius=0.028; color='k'; + % knee 1 + x_center=out.pG11; y_center=out.pG12; + draw_circle(x_center,y_center,radius,color); + + % knee 2 + x_center=out.pG21; y_center=out.pG22; + draw_circle(x_center,y_center,radius,color); + + % hip + x_center=out.pH1; y_center=out.pH2; + draw_circle(x_center,y_center,radius,color); + + % Draw load + + global SimConfig + m_load=SimConfig.m_load; + rm=sqrt(m_load/12)/10; + + pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + + % Display load + figure_x_limits = [-1.5 4];%[-1.5 4]; + figure_y_limits = [-1 2.3];% + +% xt=(figure_x_limits(1)+figure_x_limits(2))/2+out.pH1+0.5; +% yt=figure_y_limits(2)-0.5; +% disp_mass=text(xt,yt,['Load=',num2str(m_load),' [kg]']); +% set(disp_mass,'FontSize',15); + +% update axes to keep hip position fixed + set(parent,'XLim',figure_x_limits+[out.pH1 out.pH1],'YLim',figure_y_limits); +end + +function draw_ground(xe, pH_horiz, l_min, l_max, k) +[n,m]=size(xe); + +lws=2; +% draw the first ground +out = limb_position(xe(1,1:5),pH_horiz(1)); +line([out.pFoot21-0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; + +% draw the last 10 stone position +km=max(1,k-10); +for j=km:k + color1=0.7*[1 1 1]; % gray +% color1=[0 0 1]; % blue + color2=[1 1 1];% white + if (k==km)||(j==k) + color='k'; % make current stone position black + else + color=color1*(j-km)/(k-km)+color2*(k-j)/(k-km); % dim out past stone position + end + + out = limb_position(xe(j,1:5),pH_horiz(j)); + line([out.pFoot11+l_min(j,1) out.pFoot11+l_max(j,1)],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color',color);hold on; + line([out.pFoot11+l_min(j,1) out.pFoot11+l_min(j,1)],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color',color);hold on; + line([out.pFoot11+l_max(j,1) out.pFoot11+l_max(j,1)],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color',color);hold on; +end + +% hold stone of previous steps +for j=1:k + out=limb_position(xe(j,1:5),pH_horiz(j)); + if jout.pFoot11)&&(out2.pFoot21 0 + theta = atan(c(2)/c(1)); +elseif c(1) < 0 + theta = atan(c(2)/c(1))+pi; +else + theta = atan2(c(2),c(1)); +end +poly = [a, R(phi+theta)*d+a, b-R(-phi+theta)*d, b, b-R(phi+theta)*d, R(-phi+theta)*d+a]; +obj=patch(poly(1,:), poly(2,:),maincolor); +set(obj,'EdgeColor',edgecolor); +end + +function obj=draw_circle(x_center,y_center,radius,color) +param=linspace(0,2*pi+2*pi/50,50); +x=radius*cos(param); +y=radius*sin(param); +obj=patch(x+x_center,y+y_center,color); +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/anim_moving_stone_load_compare.m b/demos/rabbit/anim/anim_moving_stone_load_compare.m new file mode 100644 index 0000000..f06883d --- /dev/null +++ b/demos/rabbit/anim/anim_moving_stone_load_compare.m @@ -0,0 +1,350 @@ +function anim_moving_stone_load_compare(nonrobust_data,robust_data) +% animation to compare nonrobust and robust CBF-QP +% with time varying stepping stone while carrying unknown load +% 04/05/2016 +ts=1/30; +%% load data for nonrobust control +load(nonrobust_data); +[n,m] = size(x); +n_full=n; +l_min=l_min_t(1:n); +l_max=l_max_t(1:n); + +% Estimate hip horizontal position by estimating integral of hip +% velocity +pH_horiz = zeros(n,1); +vH = hip_vel(x); % convert angles to horizontal hosition of hips +for j=2:n +pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); +end +pH_horiz_full=pH_horiz; +[te,l_min]=even_sample(t,l_min,1/ts); +[te,l_max]=even_sample(t,l_max,1/ts); +[te,pH_horiz2]=even_sample(t,pH_horiz,1/ts); +[te2,xe2]=even_sample(t,x,1/ts); +[n,m]=size(xe2); + +%% load data for robust control +load(robust_data); +[n,m] = size(x); +n_full=n; +l_min=l_min_t(1:n); +l_max=l_max_t(1:n); + +% Estimate hip horizontal position by estimating integral of hip +% velocity +pH_horiz = zeros(n,1); +vH = hip_vel(x); % convert angles to horizontal hosition of hips +for j=2:n +pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); +end +pH_horiz_full=pH_horiz; +[te,l_min]=even_sample(t,l_min,1/ts); +[te,l_max]=even_sample(t,l_max,1/ts); +[te,pH_horiz]=even_sample(t,pH_horiz,1/ts); +[te,xe]=even_sample(t,x,1/ts); +[n,m]=size(xe); + + +%save as a video +spwriter = VideoWriter('biped_walking.avi'); +set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +open(spwriter); + +fig1 = figure(1); +set(0,'Units','pixels') +scnsize = get(0,'ScreenSize'); + +screen_width = scnsize(3); +screen_height = scnsize(4); + +figure_x_limits = [-1.5 4];%[-1.5 4]; +figure_y_limits = [-1 2.3];% +% find the minimum scaling factor + +figure_x_size = figure_x_limits(2) - figure_x_limits(1); +figure_y_size = figure_y_limits(2) - figure_y_limits(1); + +xfactor = screen_width/figure_x_size; +yfactor = screen_height/figure_y_size; + +if (xfactor < yfactor) + screen_factor = 0.5*xfactor; +else + screen_factor = 0.5*yfactor; +end + +% calculate screen offsets +screen_x_offset = (screen_width - screen_factor*figure_x_size)/2; +screen_y_offset = (screen_height - screen_factor*figure_y_size)/2; + +% draw figure and axes +set(fig1,'Position', [screen_x_offset screen_y_offset screen_factor*figure_x_size screen_factor*figure_y_size]); +set(fig1,'MenuBar', 'none'); +axes1 = axes; +set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits); +set(axes1,'Position',[0 0 1 1]); +set(axes1,'Color','w'); +set(axes1,'TickDir','out'); + +for k = 1:n +% draw robot with nonrobust ctrl + % dim out robot color + cr=[1 0 0]; cb=[0 0 1]; cg=[0 1 0]; + gray=0.7*[1 1 1]; alpha=0.3; + color{1}=cr*alpha+gray*(1-alpha); + color{2}=cg*alpha+gray*(1-alpha); + color{3}=cb*alpha+gray*(1-alpha); + + % animate how robot fall + k2=length(te2); + if k<=k2 + q2=xe2(k,1:5); + out2 = limb_position(q2,pH_horiz2(k)); + drawone_robot_only(axes1,out2,color); + elseif k <=k2+10 + q2=xe2(k2,1:5)*0.0001*(k-k2); + out2 = limb_position(q2,pH_horiz2(k2)); + drawone_robot_only(axes1,out2,color); + else + q2=xe2(k2,1:5)*0.0001*10; + out2 = limb_position(q2,pH_horiz2(k2)); + drawone_robot_only(axes1,out2,color); + end + + % display ctrl name + xt=out2.pH1-1.2; + yt=figure_y_limits(2)-0.75; + disp_ctrl2=text(xt,yt,'Non-Robust'); + set(disp_ctrl2,'FontSize',14); + set(disp_ctrl2,'color',0.4*[1 1 1]); + +% draw robot with robust ctrl + drawone(axes1, xe, pH_horiz, l_min, l_max, k); + drawnow; + hold on; + pause(ts); + %pause(0.5); + frame = getframe(gcf); + writeVideo(spwriter, frame); +end +end + +function drawone(parent, xe, pH_horiz, l_min, l_max, k) +% delete previous screen +% tem = get(parent,'Children'); +% delete(tem); + +leg1_color='r'; +leg2_color='g'; +torso_color='b'; + +[n,m]=size(xe); + +out = limb_position(xe(k,1:5),pH_horiz(k)); + +%draw stones + draw_ground(xe, pH_horiz, l_min, l_max, k); + +% stance leg + % tibia1: Foot 1 to G1 + a=[out.pFoot11;out.pFoot12];b=[out.pG11;out.pG12]; + pt_link2(a,b,leg1_color,leg1_color); + + % femur 1: H to G1 + a=[out.pG11;out.pG12];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg1_color,leg1_color); + +% swing leg + % tibia2: Foot 2 to G2 + a=[out.pFoot21;out.pFoot22];b=[out.pG21;out.pG22]; + pt_link2(a,b,leg2_color,leg2_color); + + % femur 2: H to G2 + a=[out.pG21;out.pG22];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg2_color,leg2_color); + + +% torso: Head to H + a=[out.pH1;out.pH2];b=[out.pHead1;out.pHead2]; + pt_link2(a,b,torso_color,torso_color); + + +% joints + radius=0.028; color='k'; + % knee 1 + x_center=out.pG11; y_center=out.pG12; + draw_circle(x_center,y_center,radius,color); + + % knee 2 + x_center=out.pG21; y_center=out.pG22; + draw_circle(x_center,y_center,radius,color); + + % hip + x_center=out.pH1; y_center=out.pH2; + draw_circle(x_center,y_center,radius,color); + + % Draw load + + global SimConfig + m_load=SimConfig.m_load; + rm=sqrt(m_load/12)/10; + + pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + + % Display load + figure_x_limits = [-1.5 4];%[-1.5 4]; + figure_y_limits = [-1 2.3];% + + xt=(figure_x_limits(1)+figure_x_limits(2))/2+out.pH1+0.9; + yt=figure_y_limits(2)-0.4; + disp_mass=text(xt,yt,['m_{load}=',num2str(m_load),' [kg](47%)']); + set(disp_mass,'FontSize',14); + + % Display ctrl name + xt=out.pH1+0.5; + yt=figure_y_limits(2)-0.75; + disp_ctrl1=text(xt,yt,'Robust'); + set(disp_ctrl1,'FontSize',14); + + % update axes to keep hip position fixed + set(parent,'XLim',figure_x_limits+[out.pH1 out.pH1],'YLim',figure_y_limits); +end + +function drawone_robot_only(parent, out, robot_color) +% delete previous screen +tem = get(parent,'Children'); +delete(tem); + +leg1_color=robot_color{1}; +leg2_color=robot_color{2}; +torso_color=robot_color{3}; + +% stance leg + % tibia1: Foot 1 to G1 + a=[out.pFoot11;out.pFoot12];b=[out.pG11;out.pG12]; + pt_link2(a,b,leg1_color,leg1_color); + + % femur 1: H to G1 + a=[out.pG11;out.pG12];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg1_color,leg1_color); + +% swing leg + % tibia2: Foot 2 to G2 + a=[out.pFoot21;out.pFoot22];b=[out.pG21;out.pG22]; + pt_link2(a,b,leg2_color,leg2_color); + + % femur 2: H to G2 + a=[out.pG21;out.pG22];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg2_color,leg2_color); + + +% torso: Head to H + a=[out.pH1;out.pH2];b=[out.pHead1;out.pHead2]; + pt_link2(a,b,torso_color,torso_color); + + +% joints + radius=0.028; color='k'; + % knee 1 + x_center=out.pG11; y_center=out.pG12; + draw_circle(x_center,y_center,radius,color); + + % knee 2 + x_center=out.pG21; y_center=out.pG22; + draw_circle(x_center,y_center,radius,color); + + % hip + x_center=out.pH1; y_center=out.pH2; + draw_circle(x_center,y_center,radius,color); + + % Draw load + + global SimConfig + m_load=SimConfig.m_load; + rm=sqrt(m_load/12)/10; + + pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); +end + +function draw_ground(xe, pH_horiz, l_min, l_max, k) +[n,m]=size(xe); + +lws=2; +% draw the first ground +out = limb_position(xe(1,1:5),pH_horiz(1)); +line([out.pFoot21-0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; + +% draw the last 10 stone position +km=max(1,k-10); +for j=km:k + color1=0.7*[1 1 1]; % gray +% color1=[0 0 1]; % blue + color2=[1 1 1];% white + if (k==km)||(j==k) + color='k'; % make current stone position black + else + color=color1*(j-km)/(k-km)+color2*(k-j)/(k-km); % dim out past stone position + end + + out = limb_position(xe(j,1:5),pH_horiz(j)); + line([out.pFoot11+l_min(j,1) out.pFoot11+l_max(j,1)],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color',color);hold on; + line([out.pFoot11+l_min(j,1) out.pFoot11+l_min(j,1)],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color',color);hold on; + line([out.pFoot11+l_max(j,1) out.pFoot11+l_max(j,1)],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color',color);hold on; +end + +% hold stone of previous steps +for j=1:k + out=limb_position(xe(j,1:5),pH_horiz(j)); + if jout.pFoot11)&&(out2.pFoot21 0 + theta = atan(c(2)/c(1)); +elseif c(1) < 0 + theta = atan(c(2)/c(1))+pi; +else + theta = atan2(c(2),c(1)); +end +poly = [a, R(phi+theta)*d+a, b-R(-phi+theta)*d, b, b-R(phi+theta)*d, R(-phi+theta)*d+a]; +obj=patch(poly(1,:), poly(2,:),maincolor); +set(obj,'EdgeColor',edgecolor); +end + +function obj=draw_circle(x_center,y_center,radius,color) +param=linspace(0,2*pi+2*pi/50,50); +x=radius*cos(param); +y=radius*sin(param); +obj=patch(x+x_center,y+y_center,color); +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/bar_percent.m b/demos/rabbit/anim/bar_percent.m new file mode 100644 index 0000000..836a551 --- /dev/null +++ b/demos/rabbit/anim/bar_percent.m @@ -0,0 +1,64 @@ +close all; + +load('data/all_tests_combined_ctrl10_test_file_Aug_18') +A=all_tests.stone_success;A10=sum(A(:)); + +load('data/all_tests_combined_ctrl11_test_file_Aug_18') +A=all_tests.stone_success;A11=sum(A(:)); + +load('data/all_tests_combined_ctrl12_test_file_Aug_18') +A=all_tests.stone_success;A12=sum(A(:)); + +load('data/all_tests_combined_ctrl13_test_file_Aug_18') +A=all_tests.stone_success;A13=sum(A(:)); + +% ctrl1='CBF-CLF-QP'; +% ctrl2='CBF-CLF-QP with Constraints'; +% ctrl3='Robust CBF-CLF-QP'; +% ctrl4='Robust CBF-CLF-QP with Robust Constraints'; + +x=[1 ;2;3;4];y=[A10;A12;A11;A13]; +ctrl1='I'; +ctrl2='II'; +ctrl3='III'; +ctrl4='IV'; + +c1='r';c2='y'; c3='g'; c4='b'; + +figure; +b=bar(1,A10); +set(b,'facecolor',c1,'edgecolor',c1); +% set(gca,'XTickLabel', {'CLF-FSL-QP'}); +hold on; +% b=bar([2 s11]); +b=bar(2,A12); +set(b,'facecolor',c2,'edgecolor',c2); + +b=bar(3,A11); +set(b,'facecolor',c3,'edgecolor',c3); + +b=bar(4,A13); +set(b,'facecolor',c4,'edgecolor',c4); + +% xlabel('Simulation Number'); +ylabel('Percentage of Sucessful Tests'); +ylim([0 100]); +set(gca,'XTickLabel', ''); +% text(x,y,[num2str(y) '%'],... +% 'HorizontalAlignment','center',... +% 'VerticalAlignment','bottom'); +text(x,y,{[num2str(y(1)) '%'],[num2str(y(2)) '%'],... + [num2str(y(3)) '%'],[num2str(y(4)) '%']},... + 'HorizontalAlignment','center',... + 'VerticalAlignment','bottom'); +num2str(y) +text(x,-8*[1;1;1;1],{ctrl1;ctrl2;ctrl3;ctrl4},... + 'HorizontalAlignment','center',... + 'VerticalAlignment','bottom'); +box off; + +savename=['figures/all_tests_percent_4ctrls']; +pos=[0 0 9.69/2 2.5]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf'); \ No newline at end of file diff --git a/demos/rabbit/anim/coordinateTransformation.m b/demos/rabbit/anim/coordinateTransformation.m new file mode 100644 index 0000000..8edc862 --- /dev/null +++ b/demos/rabbit/anim/coordinateTransformation.m @@ -0,0 +1,15 @@ +function s_quan_vec = coordinateTransformation(s_vec) +% ours to quans +qrel = s_vec(:, 3:7)'; +dqrel = s_vec(:, 10:14)'; + +T = [-1 -1 0 0 0; + -1 0 0 -1 0; + -1 -1 -1 0 0; + -1 0 0 -1 -1; + -1 0 0 0 0]; + +q = T*qrel + 2*pi*[ones(4,1);0]; +dq = T*dqrel; +s_quan_vec = [q; dq]'; +end \ No newline at end of file diff --git a/demos/rabbit/anim/even_sample.m b/demos/rabbit/anim/even_sample.m new file mode 100644 index 0000000..dbdcde2 --- /dev/null +++ b/demos/rabbit/anim/even_sample.m @@ -0,0 +1,42 @@ +%*********************************************************************** +% +% CONVERTS A RANDOMLY SAMPLED SIGNAL SET INTO AN EVENLY SAMPLED +% SIGNAL SET (by interpolation) +% +% By : Haldun KOMSUOGLU +% Start : 07/23/1999 +% Last : 07/23/1999 +% Statue : Neural Model Research Material +% +% Inputs: +% t : A column vector that contains the time values for the +% corresponding computed state trajectory points +% x : A matrix in which each row is a state value on the +% solution trajectory that corresponds to the time value in +% vector t with the same row value. Format of each row: +% row = [x1 x2 ... xN] +% Fs: The sampling frequency (1/sec) for the evenly sampled +% set to be generated. +% +% Outputs: +% Et : Even sampling instants. This is a column vector in the +% same format with "t". +% Ex : A matrix of the same form with "x" that contains the +% state values corresponding to the time instants in Et +% +%*********************************************************************** +function [Et, Ex] = even_sample(t, x, Fs) +% Obtain the process related parameters +N = size(x, 2); % number of signals to be interpolated +M = size(t, 1); % Number of samples provided +t0 = t(1,1); % Initial time +tf = t(M,1); % Final time +EM = (tf-t0)*Fs; % Number of samples in the evenly sampled case with + % the specified sampling frequency +Et = linspace(t0, tf, EM)'; + +% Using linear interpolation (used to be cubic spline interpolation) +% and re-sample each signal to obtain the evenly sampled forms +for s = 1:N + Ex(:,s) = interp1(t(:,1), x(:,s), Et(:,1)); +end \ No newline at end of file diff --git a/demos/rabbit/anim/make_fig_contact_force.m b/demos/rabbit/anim/make_fig_contact_force.m new file mode 100644 index 0000000..2064a82 --- /dev/null +++ b/demos/rabbit/anim/make_fig_contact_force.m @@ -0,0 +1,41 @@ +function make_fig_contact_force(t,x,u) +global m_scale_factor j_scale_factor SimConfig +scale=SimConfig.m_load/SimConfig.m_torso+1; m_scale_factor=scale; j_scale_factor=scale; +N=length(t); +contact_force=zeros(N,2); +% m_robot=1;%plot accel_com only%32+m_load; +m_robot=32+SimConfig.m_load; +g=9.81; +for i=1:N +q=x(i,1:5)';dq=x(i,6:10)'; +[D,C,G,B]=dyn_mod_abs_red(x(i,:)); +ddq = inv(D)*[-C*dq-G+B*u(i,:)']; +[com_pos , com_vel , com_accel] = COM_pos_vel_accel(q, dq, ddq); +contact_force(i,:)=m_robot*([0;g]+com_accel)';%m_robot*(g+com_accel(2)); +end + +figure(10); +plot(t,contact_force(:,2),'linewidth',2); +ylabel('N (N)'); +xlabel('Time (s)'); + +savename='figures\contact_force'; + +pos=[0 0 9.69/2 2]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') + +figure(11); +mu=contact_force(:,1)./contact_force(:,2); +plot(t,abs(mu),'linewidth',2); +ylabel('|F/N|'); +xlabel('Time (s)'); + +savename='figures\friction'; + +pos=[0 0 9.69/2 2]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') +end diff --git a/demos/rabbit/anim/make_fig_step_length.m b/demos/rabbit/anim/make_fig_step_length.m new file mode 100644 index 0000000..32b0167 --- /dev/null +++ b/demos/rabbit/anim/make_fig_step_length.m @@ -0,0 +1,30 @@ +function [] = make_fig_step_length(t,x,l_min_t,l_max_t) + +% close all; + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = modelParameters; +q1=x(:,1);q2=x(:,2);q3=x(:,3);q4=x(:,4); + +lf=L3*sin(q1)+L4*sin(q3)-L3*sin(q2)-L4*sin(q4); + +global SimConfig +controller=SimConfig.controller; +l_min=controller.CBF_l_min; +l_max=controller.CBF_l_max; + +figure(1); +h=plot(t,lf, t,l_min_t',t,l_max_t'); +set(h(1),'linewidth',2); +% title('Step Length'); +xlabel('Time (s)'); +ylabel('[m]'); +legend('l_s','l_{min}','l_{max}','Location','northoutside','Orientation','horizontal'); +axis tight; + +savename='figures\step_length'; + +pos=[0 0 9.69/2 2.5]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') + diff --git a/demos/rabbit/anim/make_fig_stick_moving_stone_load.m b/demos/rabbit/anim/make_fig_stick_moving_stone_load.m new file mode 100644 index 0000000..980bee6 --- /dev/null +++ b/demos/rabbit/anim/make_fig_stick_moving_stone_load.m @@ -0,0 +1,213 @@ +function make_fig_stick_moving_stone_load(t,x,l_min_t,l_max_t,N_stones) + +ts=1/10; + +[n,m] = size(x); +n_full=n; +l_min=l_min_t(1:n); +l_max=l_max_t(1:n); + +% Estimate hip horizontal position by estimating integral of hip +% velocity +pH_horiz = zeros(n,1); +vH = hip_vel(x); % convert angles to horizontal hosition of hips +for j=2:n +pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); +end +pH_horiz_full=pH_horiz; +[te,l_min]=even_sample(t,l_min,1/ts); +[te,l_max]=even_sample(t,l_max,1/ts); +[te,pH_horiz]=even_sample(t,pH_horiz,1/ts); +[te,xe]=even_sample(t,x,1/ts); +[n,m]=size(xe); + +fig1 = figure(1); + +axes1 = axes; +% set(axes,'Color','w'); +% set(axes,'TickDir','out'); +axis equal; +axis off; +axis tight; + + +% draw first robot +drawone(axes1, xe, pH_horiz, l_min, l_max, 1); + +% draw moving stones +draw_ground(xe, pH_horiz, l_min, l_max, N_stones); +hold on; + +% save fig +savename='figures\stick_fig'; + +pos=[0 0 9.69/2 2]*2; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') +% saveas(gcf, savename, 'png') +% print('-f1','-depsc','-painters',savename); +end + +function drawone(parent, xe, pH_horiz, l_min, l_max, k) +% delete previous screen +% tem = get(parent,'Children'); +% delete(tem); + +leg1_color='g'; +leg2_color='r'; +torso_color='b'; + +[n,m]=size(xe); + +out = limb_position(xe(k,1:5),pH_horiz(k)); + +%draw stones +% draw_ground(xe, pH_horiz, l_min, l_max, k); + +% stance leg + % tibia1: Foot 1 to G1 + a=[out.pFoot11;out.pFoot12];b=[out.pG11;out.pG12]; + pt_link2(a,b,leg1_color,leg1_color); + + % femur 1: H to G1 + a=[out.pG11;out.pG12];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg1_color,leg1_color); + +% swing leg + % tibia2: Foot 2 to G2 + a=[out.pFoot21;out.pFoot22];b=[out.pG21;out.pG22]; + pt_link2(a,b,leg2_color,leg2_color); + + % femur 2: H to G2 + a=[out.pG21;out.pG22];b=[out.pH1;out.pH2]; + pt_link2(a,b,leg2_color,leg2_color); + + +% torso: Head to H + a=[out.pH1;out.pH2];b=[out.pHead1;out.pHead2]; + pt_link2(a,b,torso_color,torso_color); + + +% joints + radius=0.028; color='k'; + % knee 1 + x_center=out.pG11; y_center=out.pG12; + draw_circle(x_center,y_center,radius,color); + + % knee 2 + x_center=out.pG21; y_center=out.pG22; + draw_circle(x_center,y_center,radius,color); + + % hip + x_center=out.pH1; y_center=out.pH2; + draw_circle(x_center,y_center,radius,color); + + % Draw load + + global SimConfig + m_load=SimConfig.m_load; + rm=sqrt(m_load/12)/10; + + pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + +% % Display load +% figure_x_limits = [-1.5 4];%[-1.5 4]; +% figure_y_limits = [-1 2.3];% +% +% xt=(figure_x_limits(1)+figure_x_limits(2))/2+out.pH1+0.5; +% yt=figure_y_limits(2)-0.5; +% disp_mass=text(xt,yt,['Load=',num2str(m_load),' [kg]']); +% set(disp_mass,'FontSize',15); +end + +function draw_ground(xe, pH_horiz, l_min, l_max,N_stones) +[n,m]=size(xe); + +lws=2; +% draw the first ground +out = limb_position(xe(1,1:5),pH_horiz(1)); +line([out.pFoot21-0.2 out.pFoot11+0.15],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.15 out.pFoot11+0.15],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; + + +%% determine step count +step_count=1; +step_start_index(1)=1; +for j=1:n + out=limb_position(xe(j,1:5),pH_horiz(j)); + if jout.pFoot11)&&(out2.pFoot21 0 + theta = atan(c(2)/c(1)); +elseif c(1) < 0 + theta = atan(c(2)/c(1))+pi; +else + theta = atan2(c(2),c(1)); +end +poly = [a, R(phi+theta)*d+a, b-R(-phi+theta)*d, b, b-R(phi+theta)*d, R(-phi+theta)*d+a]; +obj=patch(poly(1,:), poly(2,:),maincolor); +set(obj,'EdgeColor',edgecolor); +end + +function obj=draw_circle(x_center,y_center,radius,color) +param=linspace(0,2*pi+2*pi/50,50); +x=radius*cos(param); +y=radius*sin(param); +obj=patch(x+x_center,y+y_center,color); +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/make_plots.m b/demos/rabbit/anim/make_plots.m new file mode 100644 index 0000000..69eeeb7 --- /dev/null +++ b/demos/rabbit/anim/make_plots.m @@ -0,0 +1,433 @@ +function [] = make_plots(t,x,t2,u,y,dy,f,V,Vdot,v,step_t,doplots,doprints,print_type) +%MAKE_PLOTS Make plots of walking. +% +% See also WALK. + +%Eric Westervelt +%8/21/00 + +close all %KSG [01MAY12] + +if nargin < 14 + print_type = 'ps'; +end + +if nargin < 13 + doprints = 0; +end + +if nargin < 12 + doplots = ones(9,1); doplots(end)=0; +end + +if length(doplots) < 6 + error('Need to specify whether to plot EACH plot.') +end + +figno = 2; +plot_num = 0; + +offset = [50 50 0 0]; +screen_position = [30 100 300 400]; +paper_position = [1.25 1.5 6 8]; + +plot_num = plot_num + 1; +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(2,1,1) + plot(t2,v) + legend('v_{31}','v_{32}','v_{41}','v_{42}',-1) + title('Added control effort v') + xlabel('time (sec)') + grid + axis([min(t2) max(t2) min(min(v))*1.1 max(max(v))*1.1]) + subplot(2,1,2) + u_star = u-v; + plot(t2,u_star) + legend('u^{*}_{31}','u^{*}_{32}','u^{*}_{41}','u^{*}_{42}',-1) + title('u^{*} feedforward control') + xlabel('time (sec)') + grid + axis([min(t2) max(t2) min(min(u_star))*1.1 max(max(u_star))*1.1]) + if doprints + name = 'v_and_u_star'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + +plot_num = plot_num + 1; +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + +% subplot(2,1,1) +% plot(t2,V) +% legend('V_{\epsilon}') +% title('Control Lyapunov Function V_{\epsilon}') +% xlabel('time (sec)') +% grid +% axis([min(t2) max(t2) min(min(V))*1.1 max(max(V))*1.1]) +% subplot(2,1,2) +% plot(t2,Vdot) +% legend('dV_{\epsilon}') +% title('Time Derivative of Control Lyapunov Function V_{\epsilon}') +% xlabel('time (sec)') +% grid +% axis([min(t2) max(t2) min(min(Vdot))*1.1 max(max(Vdot))*1.1]) + + subplot(2,1,1) + V_to_plot = V/V(1); %Either plot V or plot V/V(1) + plot(t2,V_to_plot) + legend('dV_{\epsilon}') + title('Control Lyapunov Function V_{\epsilon} normalized by initial value') + xlabel('time (sec)') + grid + axis([min(t2) max(t2) min(min(V_to_plot))*1.1 max(max(V_to_plot))*1.1]) + subplot(2,1,2) + Vdot_to_plot = Vdot/abs(Vdot(1)); + plot(t2,Vdot_to_plot) + legend('dV_{\epsilon}') + title('Time Derivative of Control Lyapunov Function V_{\epsilon} normalized by initial value') + xlabel('time (sec)') + grid + axis([min(t2) max(t2) min(min(Vdot_to_plot))*1.1 max(max(Vdot_to_plot))*1.1]) + if doprints + name = 'CLF'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + +% plot_num = plot_num + 1; +% if doplots(plot_num) +% figure +% plot(0:.01:10,sin(0:.01:10)) +% end + +plot_num = plot_num + 1; +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(3,1,1) + plot(t,x(:,1),'-',t,x(:,2),'--') + legend('q_{31}','q_{32}',-1) + grid + xlabel('time (sec)') + axis([min(t) max(t) min(min(x(:,1:2)))*.99 max(max(x(:,1:2)))*1.01]) + subplot(3,1,2) + plot(t,x(:,3),'-',t,x(:,4),'--',t,x(:,5),'-.') + legend('q_{41}','q_{42}',-1) + xlabel('time (sec)') + grid + axis([min(t) max(t) min(min(x(:,3:4)))*.99 max(max(x(:,3:4)))*1.01]) + subplot(3,1,3) + plot(t,x(:,5)) + legend('q_1',-1) + xlabel('time (sec)') + grid + % whos + % [min(t) max(t) min(x(:,5))*1.1 max(x(:,5))*.9] + % axis([min(t) max(t) min(x(:,5))*1.1 max(x(:,5))*.9]) + axis([min(t) max(t) min(x(:,5)) max(x(:,5))]) + + if doprints + name = 'positions'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']); + else + print('-djpeg',[name,'.jpg']); + end + end +end + +plot_num = plot_num + 1; +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(3,1,1) + plot(t,x(:,6),'-',t,x(:,7),'--') + legend('q_{31} (dot)','q_{32} (dot)',-1) + grid + xlabel('time (sec)') + axis([min(t) max(t) min(min(x(:,6:7)))*1.1 max(max(x(:,6:7)))*1.1]) + subplot(3,1,2) + plot(t,x(:,8),'-',t,x(:,9),'--') + legend('q_{41} (dot)','q_{42} (dot)',-1) + grid + xlabel('time (sec)') + axis([min(t) max(t) min(min(x(:,8:9)))*1.1 max(max(x(:,8:9)))*1.1]) + subplot(3,1,3) + plot(t,x(:,10)) + legend('q_1 (dot)',-1) + grid + xlabel('time (sec)') + axis([min(t) max(t) min(x(:,10))*1.1 max(x(:,10))*1.1]) + if doprints + name = 'velocity'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + +plot_num = plot_num + 1; +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(2,1,1) + plot(t2,u(:,1),'-',t2,u(:,2),'--') + legend('\tau_{31}','\tau_{32}',-1) + title('Hip controls') + xlabel('time (sec)') + grid + axis([min(t2) max(t2) min(min(u(:,1:2)))*1.1 max(max(u(:,1:2)))*1.1]) + subplot(2,1,2) + plot(t2,u(:,3),'-',t2,u(:,4),'--') + legend('\tau_{41}','\tau_{42}',-1) + title('Knee controls') + xlabel('time (sec)') + grid + axis([min(t2) max(t2) min(min(u(:,3:4)))*1.1 max(max(u(:,3:4)))*1.1]) + if doprints + name = 'control'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + +plot_num = plot_num + 1; +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + nn = 4; mm = 2; %KSG [: Originally this had mm=1. Setting it to mm=2 causes it to plot the output derivatives + + % positions + subplot(nn,mm,1) + plot(t2,y(:,1)) + title('Outputs') + ylabel('y_1 (q1-q1d)') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(y(:,1)))*1.1 max(max(y(:,1)))*1.1]); + end + subplot(nn,mm,mm+1) + plot(t2,y(:,2)) + ylabel('y_2 (d1+d2)') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(y(:,2)))*1.1 max(max(y(:,2)))*1.1]); + end + subplot(nn,mm,2*mm+1) + plot(t2,y(:,3)) + ylabel('stance leg knee') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(y(:,3))) max(max(y(:,3)))*1.1]); + end + subplot(nn,mm,3*mm+1) + plot(t2,y(:,4)) + ylabel('swing leg knee') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(y(:,4))) max(max(y(:,4)))]) + end + + if mm == 2 + % velocities + subplot(nn,mm,2) + plot(t2,dy(:,1)) + title('Output time derivatives') + ylabel('dy_1 (q1-q1d)') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(dy(:,1)))*1.1 max(max(dy(:,1)))*1.1]); + end + subplot(nn,mm,4) + plot(t2,dy(:,2)) + ylabel('dy_2 (d1+d2)') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(dy(:,2)))*1.1 max(max(dy(:,2)))*1.1]); + end + subplot(nn,mm,6) + plot(t2,dy(:,3)) + ylabel('stance leg knee') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(dy(:,3))) max(max(dy(:,3)))*1.1]); + end + subplot(nn,mm,8) + plot(t2,dy(:,4)) + ylabel('swing leg knee') + xlabel('time (sec)') + grid + if 0 + axis([min(t2) max(t2) -1e-4 1e-4]); + else + axis([min(t2) max(t2) min(min(dy(:,4))) max(max(dy(:,4)))]) + end + end + if doprints + name = 'outputs'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + +plot_num = plot_num + 1; +if doplots(plot_num) + [g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(2,1,1) + plot(t,hip_pos(x)) + ylabel('Hip horizontal position') + xlabel('time (sec)') + subplot(2,1,2) + plot(t,-L4*cos(x(:,3))-L3*cos(x(:,1))+L3*cos(x(:,2))+L4*cos(x(:,4))) + grid + xlabel('time (sec)') + ylabel('Foot height (m)') + axis([min(t) max(t) 0 ... + max(-L4*cos(x(:,3))-L3*cos(x(:,1))+L3*cos(x(:,2))+L4*cos(x(:,4)))*1.1]) + if doprints + name = 'foot_hip_traj'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + + +plot_num = plot_num + 1; +if doplots(plot_num) + [g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(2,1,1) + plot(t,-L3.*cos(x(:,1))-L4*cos(x(:,3))) + grid + xlabel('time (sec)') + title('Hip height (m)') + axis([min(t) ... + max(t) ... + min(-L3.*cos(x(:,1))-L4*cos(x(:,3)))... + max(-L3.*cos(x(:,1))-L4*cos(x(:,3)))]); + + subplot(2,1,2) + plot(t,pi+x(:,3)-x(:,1),'-',t,pi+x(:,4)-x(:,2),'--') + legend('stance leg','swing leg') + grid + xlabel('time (sec)') + title('Relative knee angles (rad)') + axis([min(t) ... + max(t) ... + min(min([pi+x(:,3)-x(:,1) pi+x(:,4)-x(:,2)]))... + max(max([pi+x(:,3)-x(:,1) pi+x(:,4)-x(:,2)]))]); + + if doprints + name = 'hip_height'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end + + +plot_num = plot_num + 1 +if doplots(plot_num) + fig_hl=figure(figno); figno = figno + 1; + set(fig_hl,'Position', screen_position); + screen_position = screen_position + offset; + set(fig_hl,'PaperPosition',paper_position) + + subplot(3,1,1) + plot(t2,f(:,1)) + ylabel('F_{tan} (N)') + axis([min(t2) max(t2) min(f(:,1))*1.05 max(f(:,1))*1.1]) + grid + subplot(3,1,2) + plot(t2,f(:,2)) + ylabel('F_{norm} (N)') + axis([min(t2) max(t2) min(f(:,2))*.9 max(f(:,2))*1.1]) + grid + subplot(3,1,3) + plot(t2,f(:,1)./f(:,2)) + ylabel('F_{tan}/F_{norm}'); + xlabel('time (sec)') + axis([min(t2) max(t2) min(f(:,1)./f(:,2))*1.1 max(f(:,1)./f(:,2))*1.1]) + grid + if doprints + name = 'stance_leg_forces'; + if strcmp(print_type,'ps') + print('-dps',[name,'.ps']) + else + print('-djpeg',[name,'.jpg']); + end + end +end +tilefigs \ No newline at end of file diff --git a/demos/rabbit/anim/make_plots_com.m b/demos/rabbit/anim/make_plots_com.m new file mode 100644 index 0000000..e06f6bc --- /dev/null +++ b/demos/rabbit/anim/make_plots_com.m @@ -0,0 +1,18 @@ +function make_plots_com(t,x) +n=10; +%% plot s +% [g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = modelParameters; +q1=x(:,1);q2=x(:,2);q3=x(:,3);q4=x(:,4);q5=x(:,5); +dq1=x(:,6);dq2=x(:,7);dq3=x(:,8);dq4=x(:,9);dq5=x(:,10); + +[a,b,m] = controlParameters; +q31=q1;q41=q3; +s=(1/2*q31+1/2*q41-b)/m; % this s was tested ! +n=n+1;figure(n); plot(t,s); title('s'); + +%% plot COM +[cmh,cmv] = center_of_mass(x); +n=n+1;figure(n); plot(cmh,cmv); title('COM traj'); axis('equal'); + +% plot COM angle arctan(cmv/cmh) +n=n+1;figure(n); com_angle=atan(cmh./cmv); plot(t,com_angle);title('COM angle'); \ No newline at end of file diff --git a/demos/rabbit/anim/make_plots_quan.m b/demos/rabbit/anim/make_plots_quan.m new file mode 100644 index 0000000..437cd0b --- /dev/null +++ b/demos/rabbit/anim/make_plots_quan.m @@ -0,0 +1,32 @@ +function [] = make_plots_quan(t,torque,y,x,V,l_min_t,l_max_t,CBF) + +close all; +figure; plot(t,torque'); title('Intputs') ; grid ; xlabel('Time (s)') ; +%figure(3) ; plot(t, y') ; title('Outputs') ; grid ; xlabel('Time (s)') ; +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = modelParameters; +q1=x(:,1);q2=x(:,2);q3=x(:,3);q4=x(:,4);q5=x(:,5); +dq1=x(:,6);dq2=x(:,7);dq3=x(:,8);dq4=x(:,9);dq5=x(:,10); +figure; plot(t,V'); title('CLF'); + +lf=L3*sin(q1)+L4*sin(q3)-L3*sin(q2)-L4*sin(q4); +hf=-L3*cos(q1)-L4*cos(q3)+L3*cos(q2)+L4*cos(q4); +global SimConfig +controller=SimConfig.controller; +l_min=controller.CBF_l_min; +l_max=controller.CBF_l_max; +R1=controller.CBF_R1; +R2=controller.CBF_R2; + +%CBF=[gb;CBF1;CBF2;CBF1_dot;CBF2_dot;CBF1_condition;CBF2_condition]; +gb1=CBF(:,1);gb2=CBF(:,2); +h1=CBF(:,3);h2=CBF(:,4); +%CBF1=CBF(:,3);CBF2=CBF(:,4);CBF1_condition=CBF(:,7);CBF2_condition=CBF(:,8); +figure; plot(t,lf,t,l_min_t',':',t,l_max_t',':');title('Step Length'); +% % figure(6); +% % subplot(3,1,1);plot(t,V);title('CLF'); +% % subplot(3,1,2);plot(t,CBF1_condition);title('CBF condition 1'); +% % subplot(3,1,3);plot(t, CBF2_condition);title('CBF condition 2'); +%figure(7); plot(t,h1,t,h2);title('upper and lower constraints'); + +%figure(6); plot(t,ls_b-ls);title('leg to stone distance'); +figure; plot(t,h1,t,h2);title('Stepping Stone Constraints'); diff --git a/demos/rabbit/anim/make_plots_quan_save_figs.m b/demos/rabbit/anim/make_plots_quan_save_figs.m new file mode 100644 index 0000000..d394a1c --- /dev/null +++ b/demos/rabbit/anim/make_plots_quan_save_figs.m @@ -0,0 +1,75 @@ +function [] = make_plots_quan_save_figs(t,torque,y,x,V,l_min_t,l_max_t,CBF) + +close all; +% figure; plot(t,torque'); title('Intputs') ; grid ; xlabel('Time (s)') ; +%figure(3) ; plot(t, y') ; title('Outputs') ; grid ; xlabel('Time (s)') ; +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = modelParameters; +q1=x(:,1);q2=x(:,2);q3=x(:,3);q4=x(:,4);q5=x(:,5); +dq1=x(:,6);dq2=x(:,7);dq3=x(:,8);dq4=x(:,9);dq5=x(:,10); +figure; plot(t,V','linewidth',2); +% ylabel('CLF'); +% legend('CLF','Location','northwest'); +xlabel('Time (s)'); +h=legend('$V(x)$'); +set(h,'Interpreter','latex'); + +fig = gcf; +fig.PaperUnits = 'inches'; +fig.PaperPosition = [0 0 5 2]; +savename='simulation\Aug_18_data\CLF'; +saveas(gcf,savename,'jpg'); + + + +lf=L3*sin(q1)+L4*sin(q3)-L3*sin(q2)-L4*sin(q4); +hf=-L3*cos(q1)-L4*cos(q3)+L3*cos(q2)+L4*cos(q4); +global SimConfig +controller=SimConfig.controller; +l_min=controller.CBF_l_min; +l_max=controller.CBF_l_max; +R1=controller.CBF_R1; +R2=controller.CBF_R2; + +%CBF=[gb;CBF1;CBF2;CBF1_dot;CBF2_dot;CBF1_condition;CBF2_condition]; +gb1=CBF(:,1);gb2=CBF(:,2); +h1=CBF(:,3);h2=CBF(:,4); +%CBF1=CBF(:,3);CBF2=CBF(:,4);CBF1_condition=CBF(:,7);CBF2_condition=CBF(:,8); +figure; f=plot(t,lf,t,l_min_t',':',t,l_max_t','--');%title('Step Length'); +set(f(1),'linewidth',2.5); +set(f(2),'linewidth',1.5); +set(f(3),'linewidth',1.5); +h=legend('$l_f$','$l_{min}$','$l_{max}$'); +set(h,'Interpreter','latex'); +set(h,'Location','northoutside','Orientation','horizontal'); +xlabel('Time (s)'); +% ylabel('Step Length'); +axis('tight'); + +fig = gcf; +fig.PaperUnits = 'inches'; +fig.PaperPosition = [0 0 4.5 2.5]; +savename='simulation\Aug_18_data\step_length'; +saveas(gcf,savename,'jpg'); + +% % figure(6); +% % subplot(3,1,1);plot(t,V);title('CLF'); +% % subplot(3,1,2);plot(t,CBF1_condition);title('CBF condition 1'); +% % subplot(3,1,3);plot(t, CBF2_condition);title('CBF condition 2'); +%figure(7); plot(t,h1,t,h2);title('upper and lower constraints'); + +%figure(6); plot(t,ls_b-ls);title('leg to stone distance'); +figure; f=plot(t,h1,t,h2); +set(f(1),'linewidth',2.5,'linestyle',':'); +set(f(2),'linewidth',1.5); +h=legend('$h_1(x)$','$h_2(x)$'); +set(h,'Interpreter','latex'); +set(h,'Location','northoutside','Orientation','horizontal'); +% set(h,'Location','northwest'); +xlabel('Time (s)'); +% ylabel('Stepping Stone Constraints'); +axis('tight'); +fig = gcf; +fig.PaperUnits = 'inches'; +fig.PaperPosition = [0 0 4.5 2.5]; +savename='simulation\Aug_18_data\stepping_stone_constraints'; +saveas(gcf,savename,'jpg'); diff --git a/demos/rabbit/anim/make_stick_fig.m b/demos/rabbit/anim/make_stick_fig.m new file mode 100644 index 0000000..90e87e9 --- /dev/null +++ b/demos/rabbit/anim/make_stick_fig.m @@ -0,0 +1,170 @@ +function make_stick_fig(t,x,l_min_t,l_max_t,ground,savename) +%to creat video with animate of MABEL stick code +% savename=['simulation\data\ctrl13_Nsteps10_test_no8_test_file_Aug_10']; +% savename=['simulation\Aug_18_data\ctrl13_Nsteps10_test_no55_test_file_Aug_18']; +% load(savename); + +% length(x) +ts=1/7; +%Eric Westervelt +%2/15/01 +a=0.2; + + +[n,m] = size(x); +n_full=n; +l_min=zeros(n,1);l_max=zeros(n,1); +for i=1:n + l_min(i,1)=l_min_t(i,1);l_max(i,1)=l_max_t(i,1); +end +pH_horiz = zeros(n,1); + +if n == 1 | m == 1 + te = t; + xe = x; + q=xe(1:5); + n = 1; +else + % Estimate hip horizontal position by estimating integral of hip + % velocity + vH = hip_vel(x); % convert angles to horizontal hosition of hips + for j=2:n + pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); + end + pH_horiz_full=pH_horiz; + [te,l_min]=even_sample(t,l_min,1/ts); + [te,l_max]=even_sample(t,l_max,1/ts); + [te,pH_horiz]=even_sample(t,pH_horiz,1/ts); + [te,xe]=even_sample(t,x,1/ts); + [n,m]=size(xe); + q=xe(1,1:5); +end + +%save as a video +% % spwriter = VideoWriter('biped_walking_obscure.avi'); +% % set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +% % open(spwriter); + +k=0; + +out = limb_position(q,pH_horiz(1)); + +fig1 = figure(1); + +axes1 = axes; +axis equal; +axis off; +axis tight; +% draw first ground +lws=1; +line([out.pFoot21-0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; +% draw pre-located stones +[Nsteps,mg]=size(ground); +ls=0; +%gray=[0.5 0.5 0.5]; +gray='k';%[0.7 0.7 0.7]; +for i=1:Nsteps + ls_min=ls+ground(i,1); + ls_max=ls_min+0.05; +lw=1; +line([out.pFoot11+ls_min out.pFoot11+ls_max],[out.pFoot12 out.pFoot12],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_min out.pFoot11+ls_min],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_max out.pFoot11+ls_max],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; +ls=ls+ground(i,2); +end + +drawone(out,1); + +leg=1; +for k = 1:length(te);%n + if n == 1 | m == 1, q=xe(1:5); else, q=xe(k,1:5); end + + out = limb_position(q,pH_horiz(k)); +drawone(out,leg); + if k>1 + q0=xe(k-1,1:5); + out0= limb_position(q0,pH_horiz(k-1)); +% if abs(out.pFoot11-out0.pFoot11)<0.1 +% leg=leg; +% else +% leg=rem(leg+1,2); +% % drawone(out,leg); +% end + end +end + + +pos=[0 0 9.69/2 1.5]*1; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') +end + +function drawone(out,leg) + % Use actual relations between masses in animation + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; +scl=0.022; % factor to scale masses +mr_knees=M4^(1/3)*scl; % radius of mass for knees +mr_femurs=M3^(1/3)*scl; % radius of mass for femurs +mr_torso=M1^(1/3)*scl; % radius of mass for torso +if leg +leg1_color='g'; +leg2_color='r'; +else + leg1_color='r'; +leg2_color='g'; +end +torso_color='b'; +ground_color='k'; % a.k.a. black + +% Approximate circular shape of mass +param=linspace(0,2*pi+2*pi/50,50); +xmass_knees=mr_knees*cos(param); +ymass_knees=mr_knees*sin(param); + +xmass_femurs=mr_femurs*cos(param); +ymass_femurs=mr_femurs*sin(param); + +xmass_torso=mr_torso*cos(param); +ymass_torso=mr_torso*sin(param); +% Draw stance leg +tibia1=pt_link2([out.pFoot11 ;out.pFoot12],[out.pG11 ;out.pG12],leg1_color,leg1_color); +femur1=pt_link2([out.pG11 ;out.pG12],[out.pH1; out.pH2],leg1_color,leg1_color); + + +% Draw swing leg +tibia2=pt_link2([out.pFoot21 ;out.pFoot22],[out.pG21; out.pG22],leg2_color,leg2_color); +femur2=pt_link2([out.pG21; out.pG22],[out.pH1 ;out.pH2],leg2_color,leg2_color); + +% Draw torso +torso=pt_link2([out.pH1; out.pH2],[out.pHead1 ;out.pHead2],torso_color,torso_color); + +% Draw joints +jointG1=patch(xmass_knees+out.pG11,ymass_knees+out.pG12, ... + 'k'); +jointG2=patch(xmass_knees+out.pG21,ymass_knees+out.pG22, ... + 'k'); +jointH=patch(xmass_knees+out.pH1,ymass_knees+out.pH2, ... + 'k'); + % Draw mass + global SimConfig + scale=SimConfig.m_load/12+1; + rm=sqrt(scale-1)/10; +mass=pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + + +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/plot_contact_force.m b/demos/rabbit/anim/plot_contact_force.m new file mode 100644 index 0000000..c11a246 --- /dev/null +++ b/demos/rabbit/anim/plot_contact_force.m @@ -0,0 +1,41 @@ +function plot_contact_force(t,x,u) +global m_scale_factor j_scale_factor SimConfig +scale=SimConfig.m_load/SimConfig.m_torso+1; m_scale_factor=scale; j_scale_factor=scale; +N=length(t); +contact_force=zeros(N,2); +% m_robot=1;%plot accel_com only%32+m_load; +m_robot=32+SimConfig.m_load; +g=9.81; +for i=1:N +q=x(i,1:5)';dq=x(i,6:10)'; +[D,C,G,B]=dyn_mod_abs_red(x(i,:)); +ddq = inv(D)*[-C*dq-G+B*u(i,:)']; +[com_pos , com_vel , com_accel] = COM_pos_vel_accel(q, dq, ddq); +contact_force(i,:)=m_robot*([0;g]+com_accel)';%m_robot*(g+com_accel(2)); +end + +figure(10); +plot(t,contact_force(:,2),'b','linewidth',2); +ylabel('N (N)'); +xlabel('Time(s)'); +% fig = gcf; +% fig.PaperUnits = 'inches'; +% fig.PaperPosition = [0 0 5 2]; +% savename='simulation\Aug_18_data\contact_force'; +% saveas(gcf,savename,'jpg'); + +figure(11); +mu=contact_force(:,1)./contact_force(:,2); +plot(t,abs(mu),'b','linewidth',2); +ylabel('|F/N|'); +xlabel('Time(s)'); +% fig = gcf; +% fig.PaperUnits = 'inches'; +% fig.PaperPosition = [0 0 5 2]; +% savename='simulation\Aug_18_data\friction'; +% saveas(gcf,savename,'jpg'); + +end + + + diff --git a/demos/rabbit/anim/plot_contact_force_data.m b/demos/rabbit/anim/plot_contact_force_data.m new file mode 100644 index 0000000..d47d216 --- /dev/null +++ b/demos/rabbit/anim/plot_contact_force_data.m @@ -0,0 +1,50 @@ +function plot_contact_force_data(t,x,u) +% savename=['simulation\Aug_18_data\ctrl13_Nsteps10_test_no33_test_file_Aug_18']; +% load(savename); +global m_scale_factor j_scale_factor SimConfig +scale=SimConfig.m_load/SimConfig.m_torso+1; m_scale_factor=scale; j_scale_factor=scale; +N=length(t); +contact_force=zeros(N,2); +% m_robot=1;%plot accel_com only%32+m_load; +m_robot=32+SimConfig.m_load; +g=9.81; +for i=1:N +q=x(i,1:5)';dq=x(i,6:10)'; +[D,C,G,B]=dyn_mod_abs_red(x(i,:)); +ddq = inv(D)*[-C*dq-G+B*u(i,:)']; +[com_pos , com_vel , com_accel] = COM_pos_vel_accel(q, dq, ddq); +contact_force(i,:)=m_robot*([0;g]+com_accel)';%m_robot*(g+com_accel(2)); +end + +figure(10); +plot(t,contact_force(:,2),'b','linewidth',2); +ylabel('N (N)'); +xlabel('Time(s)'); +% fig = gcf; +% fig.PaperUnits = 'inches'; +% fig.PaperPosition = [0 0 5 2]; +savename='simulation\paper_figs\contact_force'; +% saveas(gcf,savename,'jpg'); +pos=[0 0 9.6/2 2]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') + +figure(11); +mu=contact_force(:,1)./contact_force(:,2); +plot(t,abs(mu),'b','linewidth',2); +ylabel('|F/N|'); +xlabel('Time(s)'); +% fig = gcf; +% fig.PaperUnits = 'inches'; +% fig.PaperPosition = [0 0 5 2]; +savename='simulation\paper_figs\friction'; +% saveas(gcf,savename,'jpg'); +pos=[0 0 9.6/2 2]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf') +end + + + diff --git a/demos/rabbit/anim/plot_foot_traj.m b/demos/rabbit/anim/plot_foot_traj.m new file mode 100644 index 0000000..11e0fac --- /dev/null +++ b/demos/rabbit/anim/plot_foot_traj.m @@ -0,0 +1,22 @@ +function plot_foot_traj(t,x,box_height) +%PLOT_FOOT_TRAJ Plot foot trajectory with constraint box. + +%Eric Westervelt +%3/23/01 + +fig_hl = figure(1); +grid + +h = swing_foot_height(x); +Ts = t(length(t)); +Il = find(t > .1*Ts); +Iu = find(t < .96*Ts); +if Il(1) > Iu(length(Iu)) + error('check PLOT_FOOT_TRAJ.M here'); +end + +plot(t,h); +bx = t([Il(1) Iu(length(Iu)) Iu(length(Iu)) Il(1)]); +by = [0; 0; box_height; box_height]; +box = patch(bx,by,'r'); +set(box,'EdgeColor','r'); \ No newline at end of file diff --git a/demos/rabbit/anim/plot_stick_fig.m b/demos/rabbit/anim/plot_stick_fig.m new file mode 100644 index 0000000..0efa040 --- /dev/null +++ b/demos/rabbit/anim/plot_stick_fig.m @@ -0,0 +1,200 @@ +function plot_stick_fig%(t,x,ts,l_min_t,l_max_t,ground) +%to creat video with animate of MABEL stick code +% savename=['simulation\data\ctrl13_Nsteps10_test_no8_test_file_Aug_10']; +savename=['simulation\Aug_18_data\ctrl13_Nsteps10_test_no55_test_file_Aug_18']; +load(savename); + +% length(x) +ts=1/7; +%Eric Westervelt +%2/15/01 +a=0.2; + + +[n,m] = size(x); +n_full=n; +l_min=zeros(n,1);l_max=zeros(n,1); +for i=1:n + l_min(i,1)=l_min_t(i,1);l_max(i,1)=l_max_t(i,1); +end +pH_horiz = zeros(n,1); + +if n == 1 | m == 1 + te = t; + xe = x; + q=xe(1:5); + n = 1; +else + % Estimate hip horizontal position by estimating integral of hip + % velocity + vH = hip_vel(x); % convert angles to horizontal hosition of hips + for j=2:n + pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); + end + pH_horiz_full=pH_horiz; + [te,l_min]=even_sample(t,l_min,1/ts); + [te,l_max]=even_sample(t,l_max,1/ts); + [te,pH_horiz]=even_sample(t,pH_horiz,1/ts); + [te,xe]=even_sample(t,x,1/ts); + [n,m]=size(xe); + q=xe(1,1:5); +end + +%save as a video +% % spwriter = VideoWriter('biped_walking_obscure.avi'); +% % set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +% % open(spwriter); + +k=0; + +out = limb_position(q,pH_horiz(1)); + +fig1 = figure(1); + +set(0,'Units','pixels') +scnsize = get(0,'ScreenSize'); + +screen_width = scnsize(3); +screen_height = scnsize(4); + +%figure_x_limits = [-1 2];%[-1.5 4]; +%figure_y_limits = [-0.5 1];%figure_y_limits = [-.5 2.8]; +figure_x_limits = [-0.5 5];%[-1.5 4]; +figure_y_limits = [-0.1 1.5];% +% find the minimum scaling factor + + + +set(fig1,'MenuBar', 'none'); +axes1 = axes; +set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits); +set(axes1,'Position',[0 0 1 1]); +set(axes1,'Color','w'); +set(axes1,'TickDir','out'); +axis('equal'); + +% draw first ground +lws=1; +line([out.pFoot21-0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; +% draw pre-located stones +[Nsteps,mg]=size(ground); +ls=0; +%gray=[0.5 0.5 0.5]; +gray='k';%[0.7 0.7 0.7]; +for i=1:Nsteps + ls_min=ls+ground(i,1); + ls_max=ls_min+0.05; +lw=1; +line([out.pFoot11+ls_min out.pFoot11+ls_max],[out.pFoot12 out.pFoot12],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_min out.pFoot11+ls_min],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_max out.pFoot11+ls_max],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; +ls=ls+ground(i,2); +end + +drawone(out,1); + +% % % Display mass +% % xt=(figure_x_limits(1)+figure_x_limits(2))/2+out.pH1+0.5; +% % yt=figure_y_limits(2)-0.1; +% % disp_mass=text(xt,yt,['m_{load}=',num2str(SimConfig.m_load),' [kg]']); +% % set(disp_mass,'FontSize',13); + +leg=1; +for k = 1:length(te);%n + if n == 1 | m == 1, q=xe(1:5); else, q=xe(k,1:5); end + + out = limb_position(q,pH_horiz(k)); +drawone(out,leg); + if k>1 + q0=xe(k-1,1:5); + out0= limb_position(q0,pH_horiz(k-1)); +% if abs(out.pFoot11-out0.pFoot11)<0.1 +% leg=leg; +% else +% leg=rem(leg+1,2); +% % drawone(out,leg); +% end + end +end +% axis('equal'); +% fig = gcf; +% fig.PaperUnits = 'inches'; +% fig.PaperPosition = [0 0 7 4]; +% savename='simulation\Aug_18_data\stick_fig_1'; +% saveas(gcf,savename,'jpg'); +savename='simulation\paper_figs\stick_fig_1'; +set(gca,'visible','off'); +pos=[0 0 7 4]; +set(gcf, 'PaperPosition', pos); +set(gcf, 'PaperSize', pos(3:4)); +saveas(gcf, savename, 'pdf'); +end + +function drawone(out,leg) + % Use actual relations between masses in animation + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; +scl=0.022; % factor to scale masses +mr_knees=M4^(1/3)*scl; % radius of mass for knees +mr_femurs=M3^(1/3)*scl; % radius of mass for femurs +mr_torso=M1^(1/3)*scl; % radius of mass for torso +if leg +leg1_color='g'; +leg2_color='r'; +else + leg1_color='r'; +leg2_color='g'; +end +torso_color='b'; +ground_color='k'; % a.k.a. black + +% Approximate circular shape of mass +param=linspace(0,2*pi+2*pi/50,50); +xmass_knees=mr_knees*cos(param); +ymass_knees=mr_knees*sin(param); + +xmass_femurs=mr_femurs*cos(param); +ymass_femurs=mr_femurs*sin(param); + +xmass_torso=mr_torso*cos(param); +ymass_torso=mr_torso*sin(param); +% Draw stance leg +tibia1=pt_link2([out.pFoot11 ;out.pFoot12],[out.pG11 ;out.pG12],leg1_color,leg1_color); +femur1=pt_link2([out.pG11 ;out.pG12],[out.pH1; out.pH2],leg1_color,leg1_color); + + +% Draw swing leg +tibia2=pt_link2([out.pFoot21 ;out.pFoot22],[out.pG21; out.pG22],leg2_color,leg2_color); +femur2=pt_link2([out.pG21; out.pG22],[out.pH1 ;out.pH2],leg2_color,leg2_color); + +% Draw torso +torso=pt_link2([out.pH1; out.pH2],[out.pHead1 ;out.pHead2],torso_color,torso_color); + +% Draw joints +jointG1=patch(xmass_knees+out.pG11,ymass_knees+out.pG12, ... + 'k'); +jointG2=patch(xmass_knees+out.pG21,ymass_knees+out.pG22, ... + 'k'); +jointH=patch(xmass_knees+out.pH1,ymass_knees+out.pH2, ... + 'k'); + % Draw mass + global SimConfig + scale=SimConfig.m_load/12+1; + rm=sqrt(scale-1)/10; +mass=pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + + +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/plot_swing_foot_trail.m b/demos/rabbit/anim/plot_swing_foot_trail.m new file mode 100644 index 0000000..f38828f --- /dev/null +++ b/demos/rabbit/anim/plot_swing_foot_trail.m @@ -0,0 +1,226 @@ +function plot_swing_foot_trail%(t,x,ts,l_min_t,l_max_t,ground) +%to creat video with animate of MABEL stick code +% savename=['simulation\data\ctrl13_Nsteps10_test_no8_test_file_Aug_10']; +savename=['simulation\Aug_18_data\ctrl13_Nsteps10_test_no55_test_file_Aug_18']; +% load(savename); +load(savename);%,'t_2','x_2','torque','y', 'dy', 'force', 'CLF_V', 'CLF_Vdot', 'mu_added', 'SimConfig','l_min_t','l_max_t','CBF','ground'); + +% length(x) +ts=1/100; +%Eric Westervelt +%2/15/01 +a=0.2; +if nargin < 3, ts = 1/20; end + +[n,m] = size(x); +n_full=n; +l_min=zeros(n,1);l_max=zeros(n,1); +for i=1:n + l_min(i,1)=l_min_t(i,1);l_max(i,1)=l_max_t(i,1); +end +pH_horiz = zeros(n,1); + +if n == 1 | m == 1 + te = t; + xe = x; + q=xe(1:5); + n = 1; +else + % Estimate hip horizontal position by estimating integral of hip + % velocity + vH = hip_vel(x); % convert angles to horizontal hosition of hips + for j=2:n + pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); + end + pH_horiz_full=pH_horiz; + [te,l_min]=even_sample(t,l_min,1/ts); + [te,l_max]=even_sample(t,l_max,1/ts); + [te,pH_horiz]=even_sample(t,pH_horiz,1/ts); + [te,xe]=even_sample(t,x,1/ts); + [n,m]=size(xe); + q=xe(1,1:5); +end + +%save as a video +% % spwriter = VideoWriter('biped_walking_obscure.avi'); +% % set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +% % open(spwriter); + +k=0; + +out = limb_position(q,pH_horiz(1)); + +fig1 = figure(1); + +set(0,'Units','pixels') +scnsize = get(0,'ScreenSize'); + +screen_width = scnsize(3); +screen_height = scnsize(4); + +%figure_x_limits = [-1 2];%[-1.5 4]; +%figure_y_limits = [-0.5 1];%figure_y_limits = [-.5 2.8]; +figure_x_limits = [-1.5 6];%[-1.5 4]; +figure_y_limits = [-1 2.3];% +% find the minimum scaling factor + +figure_x_size = figure_x_limits(2) - figure_x_limits(1); +figure_y_size = figure_y_limits(2) - figure_y_limits(1); + +xfactor = screen_width/figure_x_size; +yfactor = screen_height/figure_y_size; + +if (xfactor < yfactor) + screen_factor = 0.5*xfactor; +else + screen_factor = 0.5*yfactor; +end + +% calculate screen offsets +screen_x_offset = (screen_width - screen_factor*figure_x_size)/2; +screen_y_offset = (screen_height - screen_factor*figure_y_size)/2; + +% draw figure and axes +set(fig1,'Position', [screen_x_offset screen_y_offset screen_factor*figure_x_size screen_factor*figure_y_size]); +% set(fig1,'MenuBar', 'none'); +axes1 = axes; +set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits); +set(axes1,'Position',[0 0 1 1]); +set(axes1,'Color','w'); +set(axes1,'TickDir','out'); + +% draw first ground +lws=2; +line([out.pFoot21-0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; +% draw pre-located stones +[Nsteps,mg]=size(ground); +ls=0; +%gray=[0.5 0.5 0.5]; +gray='k';%[0.7 0.7 0.7]; +for i=1:Nsteps + ls_min=ls+ground(i,1); + ls_max=ls_min+0.05; +% line([out.pFoot11+ls_min out.pFoot11+ls_max],[out.pFoot12 out.pFoot12],'LineWidth',0.5,'Color',gray,'LineStyle',':');hold on; +% line([out.pFoot11+ls_min out.pFoot11+ls_min],[out.pFoot12 out.pFoot12-a],'LineWidth',0.5,'Color',gray,'LineStyle',':');hold on; +% line([out.pFoot11+ls_max out.pFoot11+ls_max],[out.pFoot12 out.pFoot12-a],'LineWidth',0.5,'Color',gray,'LineStyle',':');hold on; +lw=2; +line([out.pFoot11+ls_min out.pFoot11+ls_max],[out.pFoot12 out.pFoot12],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_min out.pFoot11+ls_min],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_max out.pFoot11+ls_max],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; +ls=ls+ground(i,2); +end + +drawone(out,1); + +% Draw ground +buffer=5; +% ground=line([-buffer pH_horiz(n)+buffer],[-a -a]); +% set(ground,'Color',ground_color,'LineWidth',2); +% % for k=-buffer:floor(pH_horiz(n)+buffer) +% % ref_tick(k+buffer+1)=line([k k],[-0.07-a 0-a]); +% % set(ref_tick(k+buffer+1),'Color',ground_color); +% % ref_label(k+buffer+1)=text(-0.03+k,-0.18-a,num2str(k)); +% % set(ref_label(k+buffer+1),'FontSize',15) +% % end + +% Display mass +xt=(figure_x_limits(1)+figure_x_limits(2))/2+out.pH1+0.5; +yt=figure_y_limits(2)-0.5; +disp_mass=text(xt,yt,['m_{load}=',num2str(SimConfig.m_load),' [kg]']); + set(disp_mass,'FontSize',15); + +%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%55 +leg=1; +for k = 1:length(t);%n + if n == 1 | m == 1, q=x(1:5); else, q=x(k,1:5); end + + out = limb_position(q,pH_horiz_full(k)); + + if k>1 + q0=x(k-1,1:5); + out0= limb_position(q0,pH_horiz_full(k-1)); + if abs(out.pFoot21-out0.pFoot21)>0.15 + leg=rem(leg+1,2); +% drawone(out,leg); + end + end + % swing leg + a=[out.pFoot21;out.pFoot22]; + if leg + plot(a(1),a(2),'r.','linewidth',1); + else + plot(a(1),a(2),'g.','linewidth',1); + end + +end +end + +function drawone(out,leg) + % Use actual relations between masses in animation + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; +scl=0.022; % factor to scale masses +mr_knees=M4^(1/3)*scl; % radius of mass for knees +mr_femurs=M3^(1/3)*scl; % radius of mass for femurs +mr_torso=M1^(1/3)*scl; % radius of mass for torso +if leg +leg1_color='g'; +leg2_color='r'; +else + leg1_color='r'; +leg2_color='g'; +end +torso_color='b'; +ground_color='k'; % a.k.a. black + +% Approximate circular shape of mass +param=linspace(0,2*pi+2*pi/50,50); +xmass_knees=mr_knees*cos(param); +ymass_knees=mr_knees*sin(param); + +xmass_femurs=mr_femurs*cos(param); +ymass_femurs=mr_femurs*sin(param); + +xmass_torso=mr_torso*cos(param); +ymass_torso=mr_torso*sin(param); +% Draw stance leg +tibia1=pt_link2([out.pFoot11 ;out.pFoot12],[out.pG11 ;out.pG12],leg1_color,leg1_color); +femur1=pt_link2([out.pG11 ;out.pG12],[out.pH1; out.pH2],leg1_color,leg1_color); + + +% Draw swing leg +tibia2=pt_link2([out.pFoot21 ;out.pFoot22],[out.pG21; out.pG22],leg2_color,leg2_color); +femur2=pt_link2([out.pG21; out.pG22],[out.pH1 ;out.pH2],leg2_color,leg2_color); + +% Draw torso +torso=pt_link2([out.pH1; out.pH2],[out.pHead1 ;out.pHead2],torso_color,torso_color); + +% Draw joints +jointG1=patch(xmass_knees+out.pG11,ymass_knees+out.pG12, ... + 'k'); +jointG2=patch(xmass_knees+out.pG21,ymass_knees+out.pG22, ... + 'k'); +jointH=patch(xmass_knees+out.pH1,ymass_knees+out.pH2, ... + 'k'); + % Draw mass + global SimConfig + scale=SimConfig.m_load/12+1; + rm=sqrt(scale-1)/10; +mass=pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + + +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/plot_swing_foot_trail_full_stick.m b/demos/rabbit/anim/plot_swing_foot_trail_full_stick.m new file mode 100644 index 0000000..591b51e --- /dev/null +++ b/demos/rabbit/anim/plot_swing_foot_trail_full_stick.m @@ -0,0 +1,250 @@ +function plot_swing_foot_trail_full_stick%(t,x,ts,l_min_t,l_max_t,ground) +%to creat video with animate of MABEL stick code +% savename=['simulation\data\ctrl13_Nsteps10_test_no8_test_file_Aug_10']; +savename=['simulation\Aug_18_data\ctrl13_Nsteps10_test_no55_test_file_Aug_18']; +load(savename); + +% length(x) +ts=1/7; +%Eric Westervelt +%2/15/01 +a=0.2; + + +[n,m] = size(x); +n_full=n; +l_min=zeros(n,1);l_max=zeros(n,1); +for i=1:n + l_min(i,1)=l_min_t(i,1);l_max(i,1)=l_max_t(i,1); +end +pH_horiz = zeros(n,1); + +if n == 1 | m == 1 + te = t; + xe = x; + q=xe(1:5); + n = 1; +else + % Estimate hip horizontal position by estimating integral of hip + % velocity + vH = hip_vel(x); % convert angles to horizontal hosition of hips + for j=2:n + pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1); + end + pH_horiz_full=pH_horiz; + [te,l_min]=even_sample(t,l_min,1/ts); + [te,l_max]=even_sample(t,l_max,1/ts); + [te,pH_horiz]=even_sample(t,pH_horiz,1/ts); + [te,xe]=even_sample(t,x,1/ts); + [n,m]=size(xe); + q=xe(1,1:5); +end + +%save as a video +% % spwriter = VideoWriter('biped_walking_obscure.avi'); +% % set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +% % open(spwriter); + +k=0; + +out = limb_position(q,pH_horiz(1)); + +fig1 = figure(1); + +set(0,'Units','pixels') +scnsize = get(0,'ScreenSize'); + +screen_width = scnsize(3); +screen_height = scnsize(4); + +%figure_x_limits = [-1 2];%[-1.5 4]; +%figure_y_limits = [-0.5 1];%figure_y_limits = [-.5 2.8]; +figure_x_limits = [-1.5 6];%[-1.5 4]; +figure_y_limits = [-1 2.3];% +% find the minimum scaling factor + +figure_x_size = figure_x_limits(2) - figure_x_limits(1); +figure_y_size = figure_y_limits(2) - figure_y_limits(1); + +xfactor = screen_width/figure_x_size; +yfactor = screen_height/figure_y_size; + +if (xfactor < yfactor) + screen_factor = 0.5*xfactor; +else + screen_factor = 0.5*yfactor; +end + +% calculate screen offsets +screen_x_offset = (screen_width - screen_factor*figure_x_size)/2; +screen_y_offset = (screen_height - screen_factor*figure_y_size)/2; + +% draw figure and axes +set(fig1,'Position', [screen_x_offset screen_y_offset screen_factor*figure_x_size screen_factor*figure_y_size]); +% set(fig1,'MenuBar', 'none'); +axes1 = axes; +set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits); +set(axes1,'Position',[0 0 1 1]); +set(axes1,'Color','w'); +set(axes1,'TickDir','out'); + +% draw first ground +lws=2; +line([out.pFoot21-0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12],'LineWidth',lws,'Color','k');hold on; +line([out.pFoot11+0.2 out.pFoot11+0.2],[out.pFoot12 out.pFoot12-0.2],'LineWidth',lws,'Color','k');hold on; +% draw pre-located stones +[Nsteps,mg]=size(ground); +ls=0; +%gray=[0.5 0.5 0.5]; +gray='k';%[0.7 0.7 0.7]; +for i=1:Nsteps + ls_min=ls+ground(i,1); + ls_max=ls_min+0.05; +% line([out.pFoot11+ls_min out.pFoot11+ls_max],[out.pFoot12 out.pFoot12],'LineWidth',0.5,'Color',gray,'LineStyle',':');hold on; +% line([out.pFoot11+ls_min out.pFoot11+ls_min],[out.pFoot12 out.pFoot12-a],'LineWidth',0.5,'Color',gray,'LineStyle',':');hold on; +% line([out.pFoot11+ls_max out.pFoot11+ls_max],[out.pFoot12 out.pFoot12-a],'LineWidth',0.5,'Color',gray,'LineStyle',':');hold on; +lw=2; +line([out.pFoot11+ls_min out.pFoot11+ls_max],[out.pFoot12 out.pFoot12],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_min out.pFoot11+ls_min],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; + line([out.pFoot11+ls_max out.pFoot11+ls_max],[out.pFoot12 out.pFoot12-a],'LineWidth',lw,'Color',gray);hold on; +ls=ls+ground(i,2); +end + +drawone(out,1); + +% Draw ground +buffer=5; +% ground=line([-buffer pH_horiz(n)+buffer],[-a -a]); +% set(ground,'Color',ground_color,'LineWidth',2); +% % for k=-buffer:floor(pH_horiz(n)+buffer) +% % ref_tick(k+buffer+1)=line([k k],[-0.07-a 0-a]); +% % set(ref_tick(k+buffer+1),'Color',ground_color); +% % ref_label(k+buffer+1)=text(-0.03+k,-0.18-a,num2str(k)); +% % set(ref_label(k+buffer+1),'FontSize',15) +% % end + +% Display mass +xt=(figure_x_limits(1)+figure_x_limits(2))/2+out.pH1+0.5; +yt=figure_y_limits(2)-0.5; +disp_mass=text(xt,yt,['m_{load}=',num2str(SimConfig.m_load),' [kg]']); + set(disp_mass,'FontSize',15); + +%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%55 +leg=1; +for k = 1:length(t);%n + if n == 1 | m == 1, q=x(1:5); else, q=x(k,1:5); end + + out = limb_position(q,pH_horiz_full(k)); + + if k>1 + q0=x(k-1,1:5); + out0= limb_position(q0,pH_horiz_full(k-1)); + if abs(out.pFoot11-out0.pFoot11)<0.1 + leg=leg; + else + leg=rem(leg+1,2); +% drawone(out,leg); + end + end + % swing leg + a=[out.pFoot21;out.pFoot22]; + if leg + plot(a(1),a(2),'r.','linewidth',1); + else + plot(a(1),a(2),'g.','linewidth',1); + end + +end + +leg=1; +for k = 1:length(te);%n + if n == 1 | m == 1, q=xe(1:5); else, q=xe(k,1:5); end + + out = limb_position(q,pH_horiz(k)); +drawone(out,leg); + if k>1 + q0=xe(k-1,1:5); + out0= limb_position(q0,pH_horiz(k-1)); + if abs(out.pFoot11-out0.pFoot11)<0.1 + leg=leg; + else + leg=rem(leg+1,2); +% drawone(out,leg); + end + end +end +fig = gcf; +fig.PaperUnits = 'inches'; +fig.PaperPosition = [0 0 6 3]; +savename='simulation\Aug_18_data\stick_fig_2'; +saveas(gcf,savename,'jpg'); +end + +function drawone(out,leg) + % Use actual relations between masses in animation + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=modelParameters; +scl=0.022; % factor to scale masses +mr_knees=M4^(1/3)*scl; % radius of mass for knees +mr_femurs=M3^(1/3)*scl; % radius of mass for femurs +mr_torso=M1^(1/3)*scl; % radius of mass for torso +if leg +leg1_color='g'; +leg2_color='r'; +else + leg1_color='r'; +leg2_color='g'; +end +torso_color='b'; +ground_color='k'; % a.k.a. black + +% Approximate circular shape of mass +param=linspace(0,2*pi+2*pi/50,50); +xmass_knees=mr_knees*cos(param); +ymass_knees=mr_knees*sin(param); + +xmass_femurs=mr_femurs*cos(param); +ymass_femurs=mr_femurs*sin(param); + +xmass_torso=mr_torso*cos(param); +ymass_torso=mr_torso*sin(param); +% Draw stance leg +tibia1=pt_link2([out.pFoot11 ;out.pFoot12],[out.pG11 ;out.pG12],leg1_color,leg1_color); +femur1=pt_link2([out.pG11 ;out.pG12],[out.pH1; out.pH2],leg1_color,leg1_color); + + +% Draw swing leg +tibia2=pt_link2([out.pFoot21 ;out.pFoot22],[out.pG21; out.pG22],leg2_color,leg2_color); +femur2=pt_link2([out.pG21; out.pG22],[out.pH1 ;out.pH2],leg2_color,leg2_color); + +% Draw torso +torso=pt_link2([out.pH1; out.pH2],[out.pHead1 ;out.pHead2],torso_color,torso_color); + +% Draw joints +jointG1=patch(xmass_knees+out.pG11,ymass_knees+out.pG12, ... + 'k'); +jointG2=patch(xmass_knees+out.pG21,ymass_knees+out.pG22, ... + 'k'); +jointH=patch(xmass_knees+out.pH1,ymass_knees+out.pH2, ... + 'k'); + % Draw mass + global SimConfig + scale=SimConfig.m_load/12+1; + rm=sqrt(scale-1)/10; +mass=pt_circle([(out.pH1+out.pHead1)/2;(out.pH2+out.pHead2)/2],rm,'y'); + + +end + +function [obj]=pt_circle(pt0,rad,color) + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); +end \ No newline at end of file diff --git a/demos/rabbit/anim/pt_circle.m b/demos/rabbit/anim/pt_circle.m new file mode 100644 index 0000000..1533edc --- /dev/null +++ b/demos/rabbit/anim/pt_circle.m @@ -0,0 +1,7 @@ +function [obj]=pt_circle(pt0,rad,color,parent) + i=0:0.1:2*pi; + x=rad.*cos(i)+pt0(1); + y=rad.*sin(i)+pt0(2); + obj=patch(x,y,color,'Parent',parent); + set(obj,'EdgeColor','none'); + \ No newline at end of file diff --git a/demos/rabbit/anim/pt_circle2.m b/demos/rabbit/anim/pt_circle2.m new file mode 100644 index 0000000..ae67ce5 --- /dev/null +++ b/demos/rabbit/anim/pt_circle2.m @@ -0,0 +1,13 @@ +function [obj]=pt_circle2(pt0,rad,color) + + i=0:0.1:2*pi; + + x=rad.*cos(i)+pt0(1); + + y=rad.*sin(i)+pt0(2); + + obj=patch(x,y,color); + + set(obj,'EdgeColor','none'); + + \ No newline at end of file diff --git a/demos/rabbit/anim/pt_line.m b/demos/rabbit/anim/pt_line.m new file mode 100644 index 0000000..e62bc6a --- /dev/null +++ b/demos/rabbit/anim/pt_line.m @@ -0,0 +1 @@ +function [obj]=pt_line(pt0,pt1,color,thickness,parent) obj=line([pt0(1) pt1(1)],[pt0(2) pt1(2)],'Parent',parent); set(obj,'Color',color); set(obj,'LineWidth', thickness); \ No newline at end of file diff --git a/demos/rabbit/anim/pt_link.m b/demos/rabbit/anim/pt_link.m new file mode 100644 index 0000000..3f59298 --- /dev/null +++ b/demos/rabbit/anim/pt_link.m @@ -0,0 +1,37 @@ +function [obj]=pt_link(a,b,maincolor,edgecolor,parent) + + + +R = inline('[cos(t) -sin(t); sin(t) cos(t)]'); + +c = b-a; + + + +if c(1) > 0 + + theta = atan(c(2)/c(1)); + +elseif c(1) < 0 + + theta = atan(c(2)/c(1))+pi; + +else + + theta = atan2(c(2),c(1)); + +end + + + +d = [1/20; 0]; + +phi = pi/8; + + + +poly = [a, R(phi+theta)*d+a, b-R(-phi+theta)*d, b, b-R(phi+theta)*d, R(-phi+theta)*d+a]; + +obj=patch(poly(1,:), poly(2,:),maincolor,'Parent',parent); + +set(obj,'EdgeColor',edgecolor); \ No newline at end of file diff --git a/demos/rabbit/anim/pt_link2.m b/demos/rabbit/anim/pt_link2.m new file mode 100644 index 0000000..2c37733 --- /dev/null +++ b/demos/rabbit/anim/pt_link2.m @@ -0,0 +1,17 @@ +function [obj]=pt_link2(a,b,maincolor,edgecolor) + +R = inline('[cos(t) -sin(t); sin(t) cos(t)]'); +d = [1/20; 0]; +phi = pi/8; + +c = b-a; +if c(1) > 0 + theta = atan(c(2)/c(1)); +elseif c(1) < 0 + theta = atan(c(2)/c(1))+pi; +else + theta = atan2(c(2),c(1)); +end +poly = [a, R(phi+theta)*d+a, b-R(-phi+theta)*d, b, b-R(phi+theta)*d, R(-phi+theta)*d+a]; +obj=patch(poly(1,:), poly(2,:),maincolor); +set(obj,'EdgeColor',edgecolor); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/COM_pos_vel_accel.m b/demos/rabbit/anim/rabbit_model/COM_pos_vel_accel.m new file mode 100644 index 0000000..0413a2b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/COM_pos_vel_accel.m @@ -0,0 +1,19 @@ +function [com_pos com_vel com_accel] = COM_pos_vel_accel(q, dq, ddq) + [g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]= modelParameters; + + q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5) ; + dq1 = dq(1); dq2 = dq(2); dq3 = dq(3); dq4 = dq(4); dq5 = dq(5) ; + ddq1 = ddq(1); ddq2 = ddq(2); ddq3 = ddq(3); ddq4 = ddq(4); ddq5 = ddq(5) ; + + com_pos = [(MY1*cos(q5) - MZ3*sin(q1) - MZ3*sin(q2) - MZ4*sin(q3) - MZ4*sin(q4) - MZ1*sin(q5) + L3*M1*sin(q1) + 2*L3*M3*sin(q1) + L3*M4*sin(q1) + L4*M1*sin(q3) - L3*M4*sin(q2) + 2*L4*M3*sin(q3) + 2*L4*M4*sin(q3))/(M1 + 2*M3 + 2*M4) ; + (MZ3*cos(q1) + MZ3*cos(q2) + MZ4*cos(q3) + MZ4*cos(q4) + MZ1*cos(q5) + MY1*sin(q5) - L3*M1*cos(q1) - 2*L3*M3*cos(q1) - L3*M4*cos(q1) - L4*M1*cos(q3) + L3*M4*cos(q2) - 2*L4*M3*cos(q3) - 2*L4*M4*cos(q3))/(M1 + 2*M3 + 2*M4) ; + ] ; + + com_vel = [(dq1*(L3*M1*cos(q1) - MZ3*cos(q1) + 2*L3*M3*cos(q1) + L3*M4*cos(q1)))/(M1 + 2*M3 + 2*M4) + (dq3*(L4*M1*cos(q3) - MZ4*cos(q3) + 2*L4*M3*cos(q3) + 2*L4*M4*cos(q3)))/(M1 + 2*M3 + 2*M4) - (dq5*(MZ1*cos(q5) + MY1*sin(q5)))/(M1 + 2*M3 + 2*M4) - (dq2*(MZ3*cos(q2) + L3*M4*cos(q2)))/(M1 + 2*M3 + 2*M4) - (MZ4*dq4*cos(q4))/(M1 + 2*M3 + 2*M4) ; + (dq5*(MY1*cos(q5) - MZ1*sin(q5)))/(M1 + 2*M3 + 2*M4) + (dq1*(L3*M1*sin(q1) - MZ3*sin(q1) + 2*L3*M3*sin(q1) + L3*M4*sin(q1)))/(M1 + 2*M3 + 2*M4) + (dq3*(L4*M1*sin(q3) - MZ4*sin(q3) + 2*L4*M3*sin(q3) + 2*L4*M4*sin(q3)))/(M1 + 2*M3 + 2*M4) - (dq2*(MZ3*sin(q2) + L3*M4*sin(q2)))/(M1 + 2*M3 + 2*M4) - (MZ4*dq4*sin(q4))/(M1 + 2*M3 + 2*M4) ; + ] ; + + com_accel = [(dq2^2*(MZ3*sin(q2) + L3*M4*sin(q2)))/(M1 + 2*M3 + 2*M4) - (dq3^2*(L4*M1*sin(q3) - MZ4*sin(q3) + 2*L4*M3*sin(q3) + 2*L4*M4*sin(q3)))/(M1 + 2*M3 + 2*M4) - (dq1^2*(L3*M1*sin(q1) - MZ3*sin(q1) + 2*L3*M3*sin(q1) + L3*M4*sin(q1)))/(M1 + 2*M3 + 2*M4) + (ddq1*(L3*M1*cos(q1) - MZ3*cos(q1) + 2*L3*M3*cos(q1) + L3*M4*cos(q1)))/(M1 + 2*M3 + 2*M4) + (ddq3*(L4*M1*cos(q3) - MZ4*cos(q3) + 2*L4*M3*cos(q3) + 2*L4*M4*cos(q3)))/(M1 + 2*M3 + 2*M4) - (ddq5*(MZ1*cos(q5) + MY1*sin(q5)))/(M1 + 2*M3 + 2*M4) - (ddq2*(MZ3*cos(q2) + L3*M4*cos(q2)))/(M1 + 2*M3 + 2*M4) - (dq5^2*(MY1*cos(q5) - MZ1*sin(q5)))/(M1 + 2*M3 + 2*M4) - (MZ4*ddq4*cos(q4))/(M1 + 2*M3 + 2*M4) + (MZ4*dq4^2*sin(q4))/(M1 + 2*M3 + 2*M4) ; + (ddq5*(MY1*cos(q5) - MZ1*sin(q5)))/(M1 + 2*M3 + 2*M4) - (dq2^2*(MZ3*cos(q2) + L3*M4*cos(q2)))/(M1 + 2*M3 + 2*M4) + (ddq1*(L3*M1*sin(q1) - MZ3*sin(q1) + 2*L3*M3*sin(q1) + L3*M4*sin(q1)))/(M1 + 2*M3 + 2*M4) + (ddq3*(L4*M1*sin(q3) - MZ4*sin(q3) + 2*L4*M3*sin(q3) + 2*L4*M4*sin(q3)))/(M1 + 2*M3 + 2*M4) + (dq1^2*(L3*M1*cos(q1) - MZ3*cos(q1) + 2*L3*M3*cos(q1) + L3*M4*cos(q1)))/(M1 + 2*M3 + 2*M4) + (dq3^2*(L4*M1*cos(q3) - MZ4*cos(q3) + 2*L4*M3*cos(q3) + 2*L4*M4*cos(q3)))/(M1 + 2*M3 + 2*M4) - (ddq2*(MZ3*sin(q2) + L3*M4*sin(q2)))/(M1 + 2*M3 + 2*M4) - (dq5^2*(MZ1*cos(q5) + MY1*sin(q5)))/(M1 + 2*M3 + 2*M4) - (MZ4*ddq4*sin(q4))/(M1 + 2*M3 + 2*M4) - (MZ4*dq4^2*cos(q4))/(M1 + 2*M3 + 2*M4) ; + ] ; +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/LyapRiccatiGenerator.m b/demos/rabbit/anim/rabbit_model/LyapRiccatiGenerator.m new file mode 100644 index 0000000..2daf96f --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/LyapRiccatiGenerator.m @@ -0,0 +1,267 @@ +function [QL,QR,PL,PR] = LyapRiccatiGenerator +% This function uses optimization to generate Q and P matrices for Riccati (QR and PR) and +% Lyapunov (QL and PL) equations, such that each method will yield the same +% nominal bound for our CLF V. There are two problem types: in the first +% one, we don't fix anything a priori, and we simultaneously search for the +% Lyapunov coefficients (kp and kd to generate A, and QL) and the Riccati +% coefficients (QR). In the second problem type, we fix a particular +% Lyapunov solution and search for a QR which will yield a Riccati solution +% with the same nominal bound for the CLF V +% +% WARNING: There are some hacks here; several places where you have to +% enter the same data in a couple places + +close all +clear all +clc + +% Problem type: +% 1-> Optimize to find A, QL,QR such that the Riccati and Lyapunov solutions for P result in the correct ratios of constants +% 2-> Set A and QL, and then optimize to find QR + +problem_type = 2; + +switch problem_type + + case 1 + + nonzero_Q_corners = false; %HAVE TO SET THIS IN THE CONSTRAINT FUNCTION AS WELL + + kp_0 = .5; + kd_0 = 2; + qL_0 = 1; + qR_0 = 1; + if nonzero_Q_corners + qcL_0 = 0; + qcR_0 = 0; + end + + x0(1) = kp_0; x0(2) = kd_0; x0(3) = qL_0; x0(4) = qR_0; + if nonzero_Q_corners + x0(5) = qcL_0; x0(6) = qcR_0; + end + + options=optimset('Algorithm','active-set','Diagnostics', 'on', 'Display', 'iter'); + [x,fval,exitflag] = fmincon(@myobj,x0,[],[],[],[],[],[],@mycon1,options) + + %Check out the results + kp = x(1) + kd = x(2) + qL = x(3) + qR = x(4) + if nonzero_Q_corners + qcL = x(5) + qcR = x(6) + end + + A = [zeros(4) eye(4); + -kp*eye(4) -kd*eye(4)] ; + + QL = qL*eye(8) ; + if nonzero_Q_corners + QL(1,8) = qcL; QL(8,1) = qcL; + end + PL = lyap(A', QL) ; + c1L = min(eig(PL)); + c2L = max(eig(PL)); + c12L = c2L/c1L; + c3L = min(eig(QL))/max(eig(PL)); + + F_plain = [zeros(4), eye(4); + zeros(4), zeros(4)]; + G_plain = [zeros(4); + eye(4)]; + QR= qR*eye(8) ; + if nonzero_Q_corners + QR(1,8) = qcR; QR(8,1) = qcR; + end + PR = care(F_plain,G_plain,QR); + c1R = min(eig(PR)); + c2R = max(eig(PR)); + c12R = c2R/c1R; + c3R = min(eig(QR))/max(eig(PR)); + + case 2 + nonzero_Q_corners = true; %HAVE TO SET THIS IN THE CONSTRAINT FUNCTION AS WELL + + %Set these for a particular Lyapunov result you want to match + %%%%Bad code; you have to copy this down to mycon2 because it won't + %%%%pass it and its not global + kp = 1; + kd = 1.8; + qL = 1; + qcL = 0; + + % Initial values for optimization + qR_up_0 = 1; %First four diagonal elements of QR + qR_down_0 = .5; %Last four diagonal elements of QR + if nonzero_Q_corners + qcR_0 = 0; %Corner elements of QR + %qc2R_0 = 0; %Corner elements of QR + %qc3R_0 = 0; %Corner elements of QR + end + + + %Set up for optimization + x0(1) = qR_up_0; + x0(2) = qR_down_0; + if nonzero_Q_corners + x0(3) = qcR_0; + %x0(4) = qc2R_0; + %x0(5) = qc3R_0; + end + + options=optimset('Algorithm','active-set','Diagnostics', 'on', 'Display', 'iter'); + [x,fval,exitflag] = fmincon(@myobj,x0,[],[],[],[],[],[],@mycon2,options) + + %Check out the results + qR_up = x(1) + qR_down = x(2) + if nonzero_Q_corners + qcR = x(3) + %qc2R = x(4) + %qc3R = x(5) + end + + A = [zeros(4) eye(4); + -kp*eye(4) -kd*eye(4)] ; + + QL = qL*eye(8) ; + QL(1,8) = qcL; QL(8,1) = qcL; + PL = lyap(A', QL) ; + c1L = min(eig(PL)); + c2L = max(eig(PL)); + c12L = c2L/c1L; + c3L = min(eig(QL))/max(eig(PL)); + + F_plain = [zeros(4), eye(4); + zeros(4), zeros(4)]; + G_plain = [zeros(4); + eye(4)]; + QR= [qR_up*eye(4) zeros(4) ; zeros(4) qR_down*eye(4)]; + if nonzero_Q_corners + QR(1,8) = qcR; QR(8,1) = qcR; + %QR(1,7) = qc2R; QR(7,1) = qc2R; + %QR(2,8) = qc3R; QR(8,2) = qc3R; + end + PR = care(F_plain,G_plain,QR); + c1R = min(eig(PR)); + c2R = max(eig(PR)); + c12R = c2R/c1R; + c3R = min(eig(QR))/max(eig(PR)); +end + +end + +function f = myobj(x) +f = 1; +end + +function [c, ceq] = mycon1(x) + +nonzero_Q_corners = false; + +kp = x(1); kd = x(2); qL = x(3); qR = x(4); +if nonzero_Q_corners + qcL= x(5); qcR = x(6); +end + +A = [zeros(4) eye(4); + -kp*eye(4) -kd*eye(4)] ; + +QL = qL*eye(8) ; +if nonzero_Q_corners + QL(1,8) = qcL; QL(8,1) = qcL; +end +PL = lyap(A', QL) ; +c1L = min(eig(PL)); +c2L = max(eig(PL)); +c12L = c2L/c1L; +c3L = min(eig(QL))/max(eig(PL)); + +F_plain = [zeros(4), eye(4); + zeros(4), zeros(4)]; +G_plain = [zeros(4); + eye(4)]; +QR= qR*eye(8) ; +if nonzero_Q_corners + QR(1,8) = qcR; QR(8,1) = qcR; +end +PR = care(F_plain,G_plain,QR); +c1R = min(eig(PR)); +c2R = max(eig(PR)); +c12R = c2R/c1R; +c3R = min(eig(QR))/max(eig(PR)); + +ceq(1) = c12L-c12R; %ratio of max to min eigenvalues of each P should be the same +ceq(2) = c3L-c3R; %same gamma for each one + +%Ensure positive definite QL and QR and Hurwitz A +min_reQL = min(real(eig(QL))); +c(1) = -min_reQL + .0001; +min_reQR = min(real(eig(QR))); +c(2) = -min_reQR + .0001; +max_reA = max(real(eig(A))); +c(3) = max_reA + .0001; + +end + +function [c, ceq] = mycon2(x) + + %Set these for a particular Lyapunov result you want to match + kp = 1; + kd = 1.8; + qL = 1; + qcL = 0; + +nonzero_Q_corners = true; + + + + qR_up = x(1); + qR_down = x(2); + if nonzero_Q_corners + qcR = x(3); + %qc2R = x(4); + %qc3R = x(5); + end + + A = [zeros(4) eye(4); + -kp*eye(4) -kd*eye(4)] ; + + QL = qL*eye(8) ; + QL(1,8) = qcL; QL(8,1) = qcL; + PL = lyap(A', QL) ; + c1L = min(eig(PL)); + c2L = max(eig(PL)); + c12L = c2L/c1L; + c3L = min(eig(QL))/max(eig(PL)); + + F_plain = [zeros(4), eye(4); + zeros(4), zeros(4)]; + G_plain = [zeros(4); + eye(4)]; + QR= [qR_up*eye(4) zeros(4) ; zeros(4) qR_down*eye(4)]; + if nonzero_Q_corners + QR(1,8) = qcR; QR(8,1) = qcR; + % QR(1,7) = qc2R; QR(7,1) = qc2R; + % QR(2,8) = qc3R; QR(8,2) = qc3R; + end +PR = care(F_plain,G_plain,QR); +c1R = min(eig(PR)); +c2R = max(eig(PR)); +c12R = c2R/c1R; +c3R = min(eig(QR))/max(eig(PR)); + +ceq(1) = c12L-c12R; %ratio of max to min eigenvalues of each P should be the same +ceq(2) = c3L-c3R; %same gamma for each one + +%Ensure positive definite QL and QR and Hurwitz A +min_reQL = min(real(eig(QL))); +c(1) = -min_reQL + .0001; +min_reQR = min(real(eig(QR))); +c(2) = -min_reQR + .0001; +max_reA = max(real(eig(A))); +c(3) = max_reA + .0001; + +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/README.txt b/demos/rabbit/anim/rabbit_model/README.txt new file mode 100644 index 0000000..177c5ed --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/README.txt @@ -0,0 +1,202 @@ +Description of each m-file in the simulator. + +Eric Westervelt +4/30/01 + +(* == file is generated by another m-file in this simulator) + +Directories: +----------- +doc/ ----------------------- where all documentation is kept +mat_files/ ----------------- where all the .mat files are kept +plots/ --------------------- where all the plots are kept +closed_loop_opt/ ----------- where closed loop optimization files are + kept + + +Animation routine: +----------------- +anim.m --------------------- animation routine +limb_position.m ------------ calculate limb positions for anim.m +even_sample.m -------------- evenly sample the data vectors +* hip_vel.m ---------------- calculate the hip velocities + + +Controller design: +----------------- +symb_control.m ------------- symbolic file to generate control +* decouple_abs_red.m ------- decoupling matrix and other matrices + needed to calculate the control +bezier_obj.m --------------- objective function for doing Bezier fits +bezier_opt.m --------------- main routine for doing Bezier fits +start_of_step_ctrl_sens.m -- approximate initial control magnitude + gradients with respect to control + parameters +zd_ctrl_sens_plots_cc.m ---- the zero dynamics' vector field's + differential sensitivities to the control + parameters + + +Zero dynamics: +------------- +gen_zd.m ------------------- main zero dynamics generation routine +gen_zd_diff.m -------------- generates differential sensitivities of + zero dynamics to control parameters +gen_zd_cc.m ---------------- generates the change of coordinates to + take the zero dynamics from (theta, gamma) + coordinates to (theta, theta_dot) + coordinates +gen_zd_diff_cc.m ----------- generates differential sensitivities of + zero dynamics to control parameters in + (theta, theta_dot) coordinates +* zd.m --------------------- the zero dynamics of the biped robot in + (theta, gamma) coordinates +* zd_cc.m ------------------ the zero dynamics of the biped robot in + (theta,theta_dot) coordinates +* zd_diff.m ---------------- the zero dynamics' vector field's + differential sensitivities to the control + parameters in (theta,gamma) coordinates +* zd_diff_cc.m ------------- the zero dynamics' vector field's + differential sensitivities to the control + parameters in (theta,theta_dot) coordinates +sim_zd.m ------------------- odefile to simulate the zero dynamics in + (theta,gamma) coordinates +sim_zd_cc.m ---------------- odefile to simulate the zero dynamics in + (theta,theta_dot) coordinates +* zd_lift.m ---------------- lift from the zero dynamics states (in + (theta,theta_dot) coordinates) to the + full, 10-dimensional state +* zd_special.m ------------- lift from the zero dynamics states (in + (theta,gamma) coordinates) to the full, + 10-dimensional state for +* zd_project.m ------------- project from the full, 10-dimensional + state to the zero dynamics in + (theta,gamma) coordinates +* zd_project_cc.m ---------- project from the full, 10-dimensional + state to the zero dynamics in + (theta,theta_dot) coordinates +plot_ctrl_param_sens.m ----- plot sensitivity of zero dynamics' vector + field to one control parameter in (theta, + gamma) coordinates +plot_ctrl_param_sens_cc.m -- plot sensitivity of zero dynamics' vector + field to one control parameter in (theta, + theta_dot) coordinates + + +Full dynamics: +------------- +symb_model.m --------------- symbolic file to generate full, 7 dof, + model +symb_model_red.m ----------- symbolic file to generate reduced, 5 dof, + model +* dyn_mod_abs.m ------------ full, 7 dof, biped robot model +* dyn_mod_abs_red.m -------- reduced, 5 dof, biped robot model +main.m --------------------- routine for calling odefile (walk.m) for + simulating full dynamic model +walk.m --------------------- odefile to simulate the full model +bb.m ----------------------- Bhat-Bernstein controller +make_plots.m --------------- makes plots of full dynamic simulation +initialize.m --------------- model simulation initialization +* sigma.m ------------------ calculate the state of the robot as a + function of theta_dot at impact +* sigma_vel.m -------------- compute velocities at end of step as a + function of dtheta1 +* step_length.m ------------ calculate step length +* swing_foot_height.m ------ calculate swing foot height +* swing_foot_velocity.m ---- calculate swing foot velocity +* hip_pos.m ---------------- calculate the hip positions +* head_height.m ------------ vertical position of robot "head" (i.e., + top of torso link) +* center_of_mass.m --------- calculate center of mass of robot +impact_abs_red.m ----------- impact mapping +modelParameters.m ---------- robot model parameters (mass, link + length, etc.) +controlParameters.m -------- controller parameters +rm_gaps.m ------------------ remove integrator backstepping from data + vectors + + +Poincare map generation: +----------------------- +poincare_main.m ------------ generate Poincare return map for full + dynamics + +poincare_zd_main.m --------- generate Poincare return map for reduced + dynamics using the zero dynamics in + (theta,gamma) coordinates +poincare_zd_main_cc.m ------ generate Poincare return map for reduced + dynamics using the zero dynamics in + (theta,theta_dot) coordinates +poincare_zd_compare.m ------ compare the Poincare return map for the + full dynamics and the Poincare return map + for the reduced dynamics using the zero + dynamics in (theta,gamma) coordinates +poincare_zd_compare_cc.m --- compare the Poincare return map for the + full dynamics and the Poincare return map + for the reduced dynamics using the zero + dynamics in (theta,theta_dot) coordinates +vectfield.m ---------------- draw a two-dimensional vector field +quiver_erw.m --------------- my modified version of MATLAB's quiver + command + + +Zero dynamics closed-loop optimization & transition controller: +-------------------------------------------------------------- +compute_p1_p5.m ------------ compute transition controller's second + and second to last parameters (when using + sixth order Bezier functions) +compute_transition_ctrl.m -- compute transition controller from one + controller to another +control_synth.m ------------ calculate control signal associated with + state +plot_foot_traj.m ----------- plot swing foot trajectory with box + constraint +sim_zd_opt.m --------------- zero dynamics simulation routine for + optimization +theta_obj.m ---------------- objective function used with fzero for + calculating theta + + +Zero dynamics closed-loop optimization & transition controller (in +closed_loop/): +------------------------------------------------------------------ +controlParameters.m -------- special control parameter file for + optimization -- allows setting of control + parameters by fmincon +trans_opt.m ---------------- main optimization function for computing + transition controller (THIS IS NOT + ACTUALLY NEEDED...using compute_p1_p5.m + and averaging parameters works dandily) +trans_opt_const.m ---------- constraint function for computing + transition controller (THIS IS NOT + ACTUALLY NEEDED...using compute_p1_p5.m + and averaging parameters works dandily) +trans_opt_obj.m ------------ objective function for computing + transition controller (THIS IS NOT + ACTUALLY NEEDED...using compute_p1_p5.m + and averaging parameters works dandily) +zd_opt.m ------------------- main optimization function for + closed-loop optimization for zero + dynamics +zd_opt_const.m ------------- constraint function for closed-loop + optimization +zd_opt_obj.m --------------- objective function for closed-loop + optimization + + +Misc: +---- +fixlength.m ---------------- used in making m-files so strings don't + extend over a specified length +* total_energy_red.m ------- calculate the total energy of the biped + robot +miperhr2mpersec.m ---------- convert from miles per hour to meters per + second +mpersec2miperhr.m ---------- convert from meters per second to miles + per hour + +Not used, but interesting : +------------------------- +main_zd.m ------------------ routine for calling zd_model.mdl for + simulating zero dynamic model +zd_model.mdl --------------- Simulink model of zero-dynamics \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/bb.m b/demos/rabbit/anim/rabbit_model/bb.m new file mode 100644 index 0000000..df66a3c --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/bb.m @@ -0,0 +1,26 @@ +function [out]=BB(u,e_scl) +% +% u = (y,dy) +% +% out = (nu,v) +% +% moved from initialize.m +alpha = 0.9; +epsilon = 0.075*e_scl; +% +y=u(1); +dy=u(2); +% +dy = epsilon*dy; +phi = y+1/(2-alpha)*sign(dy)*abs(dy)^(2-alpha); +nu = (-sign(dy)*abs(dy)^alpha-sign(phi)*abs(phi)^(alpha/(2-alpha)))/epsilon^2; +% +sigma=1/2; +rho=2; +% +V = (2-alpha)/(3-alpha)*abs(phi)^((3-alpha)/(2-alpha)) + sigma*dy*phi ... + +rho/(3-alpha)*abs(dy)^(3-alpha); +if V < -1 + nu=0; +end +out=[nu,V]; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/bezier_obj.m b/demos/rabbit/anim/rabbit_model/bezier_obj.m new file mode 100644 index 0000000..b2b9b8b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/bezier_obj.m @@ -0,0 +1,93 @@ +function f = bezier_obj(param) +%BEZIER_OBJ Function to minimize while performing open-loop +% optimization. +% +% [F] = BEZIER_OBJ(PARAM) cost associated with having +% state trajectories associated with PARAM. + +%Eric Westervelt +%2/1/01 + +global C WHICH_ONE FILENAME +persistent counter iter_count + +if isempty(counter), counter = 0; end +if isempty(iter_count), iter_count = 0; end + +iter_count = iter_count + 1; +counter = counter + 1; +counter_max = 20; +if counter > counter_max + counter = counter - counter_max; +end + +data = load(['mat_files/',FILENAME]); + +[te,xe] = even_sample(data.t,data.x,1000); + +te = te - te(1); + +th1 = 1/2*(xe(:,1)+xe(:,3)); + +q31 = xe(:,1); +q32 = xe(:,2); +q41 = xe(:,3); +q42 = xe(:,4); +q1 = xe(:,5); + +switch WHICH_ONE + case 1 + traj = q1; + case 2 + traj = q32-q31; + case 3 + traj = q41-q31; + case 4 + traj = q42-q32; +end + +nt = length(traj); + +u = 1/C(1)*(th1-C(2)); + +fit = u*0; + +param_tmp = [traj(1) param traj(nt)]; + +N = length(param_tmp); +NN = N - 1; +for k = 1:N + kk = k - 1; + fit = fit + param_tmp(k)'.*factorial(NN)/ ... + factorial(kk)/ ... + factorial(NN-kk)*u.^(kk) ... + .*(1-u).^(NN-kk); +end + +f = norm(traj - fit); + +if counter == 1 + u = u*te(nt); + u_prime = linspace(0,1,N)*te(nt); + + subplot(2,2,WHICH_ONE) + plot(u,fit,u,traj,u_prime,param_tmp,'o') + line(u_prime(1:2),param_tmp(1:2),'Color','r') + line(u_prime(NN:N),param_tmp(NN:N),'Color','r') + switch WHICH_ONE + case 1 + title('Torso angle') + ylabel('q_1'); + case 2 + title('Angle between femurs') + ylabel('q_{32}-q_{31}'); + case 3 + title('Stance knee deflection') + ylabel('q_{41}-q_{31}'); + case 4 + title('Swing knee deflection') + ylabel('q_{42}-q_{32}'); + end + xlabel('t (sec)'); + drawnow +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/bezier_obj_jwg.m b/demos/rabbit/anim/rabbit_model/bezier_obj_jwg.m new file mode 100644 index 0000000..6509d01 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/bezier_obj_jwg.m @@ -0,0 +1,105 @@ +function f = bezier_obj_jwg(param) +%BEZIER_OBJ Function to minimize while performing open-loop +% optimization. +% +% [F] = BEZIER_OBJ(PARAM) cost associated with having +% state trajectories associated with PARAM. + +%Eric Westervelt +%2/1/01 + +global C WHICH_ONE FILENAME +persistent counter iter_count + +if isempty(counter), counter = 0; end +if isempty(iter_count), iter_count = 0; end + +iter_count = iter_count + 1; +counter = counter + 1; +counter_max = 20; +if counter > counter_max + counter = counter - counter_max; +end + +data = load(['mat_files/',FILENAME]); + +[te,xe] = even_sample(data.t,data.x,1000); + +te = te - te(1); + +th1 = 1/2*(xe(:,1)+xe(:,3)); +dth1=1/2*(xe(:,6)+xe(:,8)); + +q31 = xe(:,1); +q32 = xe(:,2); +q41 = xe(:,3); +q42 = xe(:,4); +q1 = xe(:,5); +dq31 = xe(:,6); +dq32 = xe(:,7); +dq41 = xe(:,8); +dq42 = xe(:,9); +dq1 = xe(:,10); + + +switch WHICH_ONE +case 1 + traj = q1-q31; + dtraj=dq1-dq31; +case 2 + traj = q1-q32; + dtraj=dq1-dq32; +case 3 + traj = q41-q31; + dtraj=dq41-dq31; +case 4 + traj = q42-q32; + dtraj=dq42-dq32; +end + +nt = length(traj); + +u = 1/C(1)*(th1-C(2)); +m=C(1); + +fit = u*0; + +param_tmp = [traj(1) traj(1)+dtraj(1)*m/(6*dth1(1)) param traj(nt)-dtraj(nt)*m/(6*dth1(nt)) traj(nt)]; + +N = length(param_tmp); +NN = N - 1; +for k = 1:N + kk = k - 1; + fit = fit + param_tmp(k)'.*factorial(NN)/ ... + factorial(kk)/ ... + factorial(NN-kk)*u.^(kk) ... + .*(1-u).^(NN-kk); +end + +f = norm(traj - fit); + +if counter == 1 + u = u*te(nt); + u_prime = linspace(0,1,N)*te(nt); + + subplot(2,2,WHICH_ONE) + plot(u,fit,u,traj,u_prime,param_tmp,'o') + line(u_prime(1:2),param_tmp(1:2),'Color','r') + line(u_prime(NN:N),param_tmp(NN:N),'Color','r') + switch WHICH_ONE + case 1 + title('Torso to femur_1 angle (done)') + ylabel('q_1-q_{31}'); + case 2 + title('Torso to femur2 (done)') + ylabel('q_1-q_{32}'); + case 3 + title('Stance knee deflection (done)') + ylabel('q_{41}-q_{31}'); + case 4 + title('Swing knee deflection (done)') + ylabel('q_{42}-q_{32}'); + end + xlabel('t (sec)'); + drawnow +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/bezier_opt.m b/demos/rabbit/anim/rabbit_model/bezier_opt.m new file mode 100644 index 0000000..c3c2d91 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/bezier_opt.m @@ -0,0 +1,138 @@ +function bezier_opt(filename) + +addpath C:\d_drive\mat_docs\robot_dir\Robot_Genoux\Westervelt_simulator_5_1_01 +addpath C:\d_drive\mat_docs\robot_dir\Robot_Genoux\Westervelt_simulator_5_1_01\mat_files + +%BEZIER_OPT +% +% Generate initial polynomial fit of state trajectories before performing +% open loop optimization. +% +%Eric Westervelt +%2/1/01 +%5/1/01 - made into a function + +global C WHICH_ONE FILENAME + +FILENAME = filename; + +data = load(['mat_files/',FILENAME]); + +[te,xe] = even_sample(data.t,data.x,1000); + +te = te - te(1); + +th1 = 1/2*(xe(:,1)+xe(:,3)); + +nt = length(th1); + +q31 = xe(:,1); +q32 = xe(:,2); +q41 = xe(:,3); +q42 = xe(:,4); +q1 = xe(:,5); + +tt = linspace(0,1,2); +phi = [tt' ones(length(tt),1)]; +C = inv(phi'*phi)*phi'*[th1(1); th1(nt)]; +u = 1/C(1)*(th1-C(2)); +%plot(th1,u) + +figure(1); clf + +for WHICH_ONE = 1:4 + switch WHICH_ONE + case 1 + traj = q1; + case 2 + traj = q32-q31; + case 3 + traj = q41-q31; + case 4 + traj = q42-q32; + end + + % change the number of parameters in this vector to change the + % number of optimiztion parameters (the total is length(a)+2) -- 7 + % seems to be best to fit to old trajectories + a = [traj(1), traj(1), traj(1), traj(nt), traj(nt)]; + %a = [traj(1), traj(1), traj(nt) traj(nt)]; % 6 params + %a = [traj(1), traj(1), traj(nt)]; % 5 params + + options = optimset('Display', 'iter',... + 'TolX', 1e-15,... + 'TolFun', 1e-6,... + 'TolCon', 1e-8,... + 'MaxFunEvals', 1e3,... + 'MaxIter', 1e2); + + [a_final_tmp,f] = fminunc('bezier_obj', a, options); + + % impose symmetry + switch WHICH_ONE + case 0 + a_final(WHICH_ONE,:) = [traj(1) a_final_tmp traj(1)]; % changed by grizzle to remove symmetry on torso + case {1,2,3,4} + a_final(WHICH_ONE,:) = [traj(1) a_final_tmp traj(nt)]; + end + + whos a_final + + fit = u*0; + + N = length(a_final(WHICH_ONE,:)); + NN = N - 1; + for k = 1:N + kk = k - 1; + fit = fit + a_final(WHICH_ONE,k)'.*factorial(NN)/ ... + factorial(kk)/ ... + factorial(NN-kk)*u.^(kk) ... + .*(1-u).^(NN-kk); + end + + th1_prime = C(1)*linspace(0,1,N)+C(2); + + if 1 + subplot(2,2,WHICH_ONE) + plot(th1,traj,th1,fit,th1_prime,a_final(WHICH_ONE,:),'o') + line(th1_prime(1:2),a_final(WHICH_ONE,1:2),'Color','r') + line(th1_prime(NN:N),a_final(WHICH_ONE,NN:N),'Color','r') + switch WHICH_ONE + case 1 + title('Torso angle (done)') + ylabel('q_1'); + case 2 + title('Angle between femurs (done)') + ylabel('q_{32}-q_{31}'); + case 3 + title('Stance knee deflection (done)') + ylabel('q_{41}-q_{31}'); + case 4 + title('Swing knee deflection (done)') + ylabel('q_{42}-q_{32}'); + end + xlabel('\theta_1 (rad)'); + drawnow; + end +end + +disp(' '); +disp('[copy this stuff to CONTROLPARAMETERS.M]'); +disp(' '); +disp([' m = ',num2str(C(1)),';']); +disp([' b = ',num2str(C(2)),';']); +disp(' '); + +a = char('a'); +for WHICH_ONE = 1:4 + % print out in a format to be copied to CONTROLPARAMETERS.M + % + b = [char(a+WHICH_ONE-1) char(a+WHICH_ONE-1)]; + disp([' ',b,' = [',num2str(a_final(WHICH_ONE,1)),'; ', ... + num2str(a_final(WHICH_ONE,2)),'; ', ... + num2str(a_final(WHICH_ONE,3)),'; ', ... + num2str(a_final(WHICH_ONE,4)),'; ', ... + num2str(a_final(WHICH_ONE,5)),'; ', ... + num2str(a_final(WHICH_ONE,6)),'; ', ... + num2str(a_final(WHICH_ONE,7)),'];']); +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/bezier_opt_jwg.m b/demos/rabbit/anim/rabbit_model/bezier_opt_jwg.m new file mode 100644 index 0000000..211b2c3 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/bezier_opt_jwg.m @@ -0,0 +1,158 @@ +function bezier_opt_jwg(filename) + + +if ~isunix +addpath C:\d_drive\mat_docs\robot_dir\Robot_Genoux\Westervelt_simulator_5_1_01 +addpath C:\d_drive\mat_docs\robot_dir\Robot_Genoux\Westervelt_simulator_5_1_01\mat_files +rep='C:\d_drive\mat_docs\robot_dir\Robot_Genoux\Westervelt_simulator_5_1_01\mat_files\'; +else +addpath /win/d_drive/mat_docs/robot_dir/Robot_Genoux/Westervelt_simulator_5_1_01 +addpath /win/d_drive/mat_docs/robot_dir/Robot_Genoux/Westervelt_simulator_5_1_01/mat_files +rep='/win/d_drive/mat_docs/robot_dir/Robot_Genoux/Westervelt_simulator_5_1_01/mat_files/'; +end + + +%BEZIER_OPT +% +% Generate initial polynomial fit of state trajectories before performing +% open loop optimization. +% +%Eric Westervelt +%2/1/01 +%5/1/01 - made into a function + +global C WHICH_ONE FILENAME + +FILENAME = filename; + +data = load(['mat_files/',FILENAME]); + +[te,xe] = even_sample(data.t,data.x,1000); + +te = te - te(1); + +th1 = 1/2*(xe(:,1)+xe(:,3)); +dth1=1/2*(xe(:,6)+xe(:,8)); + +nt = length(th1); + +q31 = xe(:,1); +q32 = xe(:,2); +q41 = xe(:,3); +q42 = xe(:,4); +q1 = xe(:,5); +dq31 = xe(:,6); +dq32 = xe(:,7); +dq41 = xe(:,8); +dq42 = xe(:,9); +dq1 = xe(:,10); + +tt = linspace(0,1,2); +phi = [tt' ones(length(tt),1)]; +C = inv(phi'*phi)*phi'*[th1(1); th1(nt)]; m=C(1); b=C(2); +u = 1/C(1)*(th1-C(2)); +%plot(th1,u) + +figure(1); clf + +for WHICH_ONE = 1:4 + switch WHICH_ONE + case 1 + traj = q1-q31; + dtraj=dq1-dq31; + case 2 + traj = q1-q32; + dtraj=dq1-dq32; + case 3 + traj = q41-q31; + dtraj=dq41-dq31; + case 4 + traj = q42-q32; + dtraj=dq42-dq32; + end + + % change the number of parameters in this vector to change the + % number of optimiztion parameters (the total is length(a)+2) -- 7 + % seems to be best to fit to old trajectories + a = [traj(1), traj(1), traj(nt)]; + %a = [traj(1), traj(1), traj(1), traj(nt), traj(nt)]; + %a = [traj(1), traj(1), traj(nt) traj(nt)]; % 6 params + %a = [traj(1), traj(1), traj(nt)]; % 5 params + + options = optimset('Display', 'iter',... + 'TolX', 1e-15,... + 'TolFun', 1e-6,... + 'TolCon', 1e-8,... + 'MaxFunEvals', 1e3,... + 'MaxIter', 1e2); + + [a_final_tmp,f] = fminunc('bezier_obj_jwg', a, options); + + % impose symmetry + switch WHICH_ONE + case 0 + a_final(WHICH_ONE,:) = [traj(1) traj(1)+dtraj(1)*m/(6*dth1(1)) a_final_tmp traj(1)-dtraj(1)*m/(6*dth1(nt)) traj(1)]; % changed by grizzle to remove symmetry on torso + case {1,2,3,4} + a_final(WHICH_ONE,:) = [traj(1) traj(1)+dtraj(1)*m/(6*dth1(1)) a_final_tmp traj(nt)-dtraj(nt)*m/(6*dth1(nt)) traj(nt)]; + end + + %whos a_final + + fit = u*0; + + N = length(a_final(WHICH_ONE,:)); + NN = N - 1; + for k = 1:N + kk = k - 1; + fit = fit + a_final(WHICH_ONE,k)'.*factorial(NN)/ ... + factorial(kk)/ ... + factorial(NN-kk)*u.^(kk) ... + .*(1-u).^(NN-kk); + end + + th1_prime = C(1)*linspace(0,1,N)+C(2); + + if 1 + subplot(2,2,WHICH_ONE) + plot(th1,traj,th1,fit,th1_prime,a_final(WHICH_ONE,:),'o') + line(th1_prime(1:2),a_final(WHICH_ONE,1:2),'Color','r') + line(th1_prime(NN:N),a_final(WHICH_ONE,NN:N),'Color','r') + switch WHICH_ONE + case 1 + title('Torso to femur_1 angle (done)') + ylabel('q_1-q_{31}'); + case 2 + title('Torso to femur2 (done)') + ylabel('q_1-q_{32}'); + case 3 + title('Stance knee deflection (done)') + ylabel('q_{41}-q_{31}'); + case 4 + title('Swing knee deflection (done)') + ylabel('q_{42}-q_{32}'); + end + xlabel('\theta_1 (rad)'); + drawnow; + end +end + +disp(' '); +disp('[copy this stuff to CONTROLPARAMETERS.M]'); +disp(' '); +disp([' m = ',num2str(C(1)),';']); +disp([' b = ',num2str(C(2)),';']); +disp(' '); + +a = char('a'); +for WHICH_ONE = 1:4 + % print out in a format to be copied to CONTROLPARAMETERS.M + % + b = [char(a+WHICH_ONE-1) char(a+WHICH_ONE-1)]; + disp([' ',b,' = [',num2str(a_final(WHICH_ONE,1)),'; ', ... + num2str(a_final(WHICH_ONE,2)),'; ', ... + num2str(a_final(WHICH_ONE,3)),'; ', ... + num2str(a_final(WHICH_ONE,4)),'; ', ... + num2str(a_final(WHICH_ONE,5)),'; ', ... + num2str(a_final(WHICH_ONE,6)),'; ', ... + num2str(a_final(WHICH_ONE,7)),'];']); +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/center_of_mass.m b/demos/rabbit/anim/rabbit_model/center_of_mass.m new file mode 100644 index 0000000..57effdf --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/center_of_mass.m @@ -0,0 +1,32 @@ +function [cmh,cmv] = center_of_mass(x) +% CENTER_OF_MASS Robot center of mass. +% [CMH,CMV] = CENTER_OF_MASS(X) + +%Eric Westervelt +%24-Mar-2001 10:46:13 + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]= modelParameters; + +[n,m] = size(x); + +if n ~= 1 & m ~= 1 + cmh = (L3.*sin(x(:,1)).*M1+L4.*sin(x(:,3)).*M1-sin(x(:,5)).*MZ1+ ... +cos(x(:,5)).*MY1+2.*L3.*sin(x(:,1)).*M3+2.*L4.*sin(x(:,3)).*M3-MZ3.* ... +sin(x(:,1))-MZ3.*sin(x(:,2))+2.*L4.*sin(x(:,3)).*M4-MZ4.*sin(x(:,3))+ ... +L3.*sin(x(:,1)).*M4-L3.*sin(x(:,2)).*M4-MZ4.*sin(x(:,4)))/(M1+2.*M3+ ... +2.*M4); + cmv = (-L3.*cos(x(:,1)).*M1-L4.*cos(x(:,3)).*M1+cos(x(:,5)).*MZ1+ ... +sin(x(:,5)).*MY1-2.*L3.*cos(x(:,1)).*M3-2.*L4.*cos(x(:,3)).*M3+MZ3.* ... +cos(x(:,1))+MZ3.*cos(x(:,2))-2.*L4.*cos(x(:,3)).*M4+MZ4.*cos(x(:,3))- ... +L3.*cos(x(:,1)).*M4+L3.*cos(x(:,2)).*M4+MZ4.*cos(x(:,4)))/(M1+2.*M3+ ... +2.*M4); +else + cmh = (L3*sin(x(1))*M1+L4*sin(x(3))*M1-sin(x(5))*MZ1+cos(x(5))*MY1+2*L3* ... +sin(x(1))*M3+2*L4*sin(x(3))*M3-MZ3*sin(x(1))-MZ3*sin(x(2))+2*L4* ... +sin(x(3))*M4-MZ4*sin(x(3))+L3*sin(x(1))*M4-L3*sin(x(2))*M4-MZ4* ... +sin(x(4)))/(M1+2*M3+2*M4); + cmv = (-L3*cos(x(1))*M1-L4*cos(x(3))*M1+cos(x(5))*MZ1+sin(x(5))*MY1-2*L3* ... +cos(x(1))*M3-2*L4*cos(x(3))*M3+MZ3*cos(x(1))+MZ3*cos(x(2))-2*L4* ... +cos(x(3))*M4+MZ4*cos(x(3))-L3*cos(x(1))*M4+L3*cos(x(2))*M4+MZ4* ... +cos(x(4)))/(M1+2*M3+2*M4); +end diff --git a/demos/rabbit/anim/rabbit_model/compute_contact_force.m b/demos/rabbit/anim/rabbit_model/compute_contact_force.m new file mode 100644 index 0000000..74161e1 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/compute_contact_force.m @@ -0,0 +1,17 @@ +function contact_force=compute_contact_force(x,u) +global m_scale_factor j_scale_factor SimConfig +scale=SimConfig.m_load/SimConfig.m_torso+1; m_scale_factor=scale; j_scale_factor=scale; + +contact_force=zeros(2,1); +m_robot=1;%plot accel_com only%32+m_load; +g=9.81; + +q=x(1:5);dq=x(6:10); +[D,C,G,B]=dyn_mod_abs_red(x); +ddq = inv(D)*[-C*dq-G+B*u]; +[com_pos , com_vel , com_accel] = COM_pos_vel_accel(q, dq, ddq); +contact_force=m_robot*([0;g]+com_accel)';%m_robot*(g+com_accel(2)); +end + + + diff --git a/demos/rabbit/anim/rabbit_model/compute_p1_p5.m b/demos/rabbit/anim/rabbit_model/compute_p1_p5.m new file mode 100644 index 0000000..a9b0386 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/compute_p1_p5.m @@ -0,0 +1,34 @@ +function [p1,p5] = compute_p1_p5(m,which_param1,which_param2) +%COMPUTE_P1_P5 Computes the (n-1)th parameters of the Bezier +% coefficients. WHICH_PARAM is for the controller to which we are +% switching. Q is the configuration of the robot at the end of the +% step. M is norminalization scaling parameter for the transition +% controller (as opposed to the controller we are switching from or +% to). DTHETA is the value of dtheta at the end of the transition +% controller's step. (See notes 4/2/01) +% +% [P1,P5] = COMPUTE_P1_P5(M,WHICH_PARAM1,WHICH_PARAM2) + +%Eric Westervelt +%4/1/01 + +global WHICH_PARAM PARAM_TMP + +param_tmp_tmp = PARAM_TMP; +PARAM_TMP = []; + +WHICH_PARAM = which_param1; +[a1,b1,m1] = controlParameters; +WHICH_PARAM = which_param2; +[a2,b2,m2] = controlParameters; + +PARAM_TMP = param_tmp_tmp; +WHICH_PARAM = []; + +p0_ctrl1 = a1([5;12;19;26]); +p1_ctrl1 = a1([6;13;20;27]); +p1 = m/m1*(-p0_ctrl1+p1_ctrl1)+p0_ctrl1; + +p5_ctrl2 = a2([10;17;24;31]); +p6_ctrl2 = a2([11;18;25;32]); +p5 = m/m2*(p5_ctrl2-p6_ctrl2)+p6_ctrl2; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/compute_transition_ctrl.m b/demos/rabbit/anim/rabbit_model/compute_transition_ctrl.m new file mode 100644 index 0000000..80cf817 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/compute_transition_ctrl.m @@ -0,0 +1,67 @@ +%COMPUTE_TRANSITION_CTRL.M + +%Eric Westervelt +%4/23/01 + +global WHICH_PARAM + +from_param = 100; +to_param = 102; + +WHICH_PARAM = from_param; +[a1,b1,m1,dtheta_fixed1] = controlParameters; + +% get velocities for matching up +x0 = sigma(dtheta_fixed1); +x0 = impact_abs_red(x0); +X_START = x0(1:10); + +WHICH_PARAM = to_param; +[a2,b2,m2,dtheta_fixed2] = controlParameters; + +% get velocities for matching up +x_end = sigma(dtheta_fixed2); +Q_END = x_end(1:5); + + +% calculate m and b scaling parameters -- they're uniquely determinted +% by imposing posture at beginning and end of step +theta_start = sum(X_START([1;3]))/2; +theta_end = sum(Q_END([1;3]))/2; +m = theta_end - theta_start; +b = theta_start; + +[p1,p5] = compute_p1_p5(m,from_param,to_param); + +params = [a1(5); p1(1); + (a1(7:9)+a2(7:9))/2; + p5(1); a2(11); + a1(12); p1(2); + (a1(14:16)+a2(14:16))/2; + p5(2); a2(18); + a1(19); p1(3); + (a1(21:23)+a2(21:23))/2; + p5(3); a2(25); + a1(26); p1(4); + (a1(28:30)+a2(28:30))/2; + p5(4); a2(32)]; + +disp('[copy this stuff to CONTROLPARAMETERS.M]'); +disp(' '); +disp([' m = ',num2str(m),';']); +disp([' b = ',num2str(b),';']); +disp(' '); + +a = char('a'); +for counter = 1:4 + % print out in a format to be copied to CONTROLPARAMETERS.M + % + b = [char(a+counter-1) char(a+counter-1)]; + disp([' ',b,' = [',num2str(params((counter-1)*7+1)),'; ', ... + num2str(params((counter-1)*7+2)),'; ', ... + num2str(params((counter-1)*7+3)),'; ', ... + num2str(params((counter-1)*7+4)),'; ',]); + disp([' ',num2str(params((counter-1)*7+5)),'; ', ... + num2str(params((counter-1)*7+6)),'; ', ... + num2str(params((counter-1)*7+7)),'];']); +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/controlParameters.m b/demos/rabbit/anim/rabbit_model/controlParameters.m new file mode 100644 index 0000000..c39a9ff --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/controlParameters.m @@ -0,0 +1,157 @@ +function [a,b,m,dtheta_fixed] = controlParameters +%CONTROLPARAMETERS Control parameters for the biped robot. + +%Eric Westervelt +%2/15/01 + +global WHICH_TO_TWEAK TWEAK_AMOUNT WHICH_PARAM + +if isempty(WHICH_PARAM) + the_param = 4; +else + the_param = WHICH_PARAM; +end + +the_param=3; +switch the_param + case 1 + % bezier fit to 100 (.75 m/s) + dtheta_fixed = -1.5258; + m = -0.58887; + b = 3.4301; + + aa = [-0.10423; -0.20055; -0.42545; + 0.048042; -0.12654; -0.088101; -0.10423]; + bb = [-0.62361; -0.85665; -0.73188; + -0.036404; 0.66029; 0.63419; 0.62361]; + cc = [-0.24094; -0.57179; -1.072; + 0.062835; -0.58463; -0.3823; -0.17162]; + dd = [-0.17154; -0.034859; -0.77177; + -0.891; -1.7761; -0.89056; -0.24118]; + case 2 + %bezier fit to 101 (.85 m/s) + dtheta_fixed = -1.4428; + m = -0.58266; + b = 3.4285; + + aa = [-0.14539; -0.26383; -0.36871; + -0.25771; -0.14864; -0.10767; -0.14539]; + bb = [-0.60976; -0.88454; -0.50656; + 0.19492; 0.4895; 0.5605; 0.60976]; + cc = [-0.22406; -0.77502; -0.76833; + -0.27294; -0.33635; -0.2637; -0.16984]; + dd = [-0.16984; -0.18134; -0.62412; + -0.85509; -1.0363; -0.61177; -0.22405]; + case 3 + %bezier fit to 102 (.95 m/s) + dtheta_fixed = -1.48705; + m = -0.57723; + b = 3.4262; + + aa = [-0.17964; -0.28762; -0.33553; + -0.44108; -0.1499; -0.14084; -0.17964]; + bb = [-0.5985; -0.89184; -0.52133; + 0.098126; 0.57312; 0.51468; 0.5985]; + cc = [-0.24245; -0.86505; -0.76354; + -0.55554; -0.12597; -0.24617; -0.19992]; + dd = [-0.19992; -0.24989; -0.88613; + -0.98059; -1.1521; -0.42793; -0.24246]; + case 4 + % fit to data_Franck2 + m = -0.64749; + b = 3.4653; + dtheta_fixed = -1.5; + aa = [0.10472; 0.086861; 0.12981; 0.084472; 0.11453; 0.10238; 0.10472]; + bb = [-0.64749; -0.99933; 0.42886; -0.35741; 0.42343; 0.43529; 0.64749]; + cc = [-0.37703; -0.70174; -0.43974; -0.878; -0.52211; -0.63045; -0.37703]; + dd = [-0.37703; -0.035692; -1.4708; -0.23464; -0.93472; -0.62639; -0.37703]; + case 100 + % best from 0019 + % peak torque 34.489 Nm ! (from 0019, #601) + % design rate = .75 m/s + % actual rate fd = .75586 m/s + % dtheta_fixed_fd = -1.5259 + % dtheta_fixed_zd = -1.5259 + + dtheta_fixed = -1.5259; + + %dtheta = -1.5244; + m = -0.5889; + b = 3.4301; + aa = [-0.104231; -0.200629; -0.425354; + 0.048028; -0.126524; -0.088105; -0.104231]; + bb = [-0.023751; -0.414821; -0.794973; + -1.050018; -0.287156; -0.048590; -0.023751]; + cc = [2.900650; 2.569582; 2.069861; + 3.204332; 2.556976; 2.759304; 2.970050]; + dd = [2.970050; 3.106941; 2.369179; + 2.251128; 1.364924; 2.251402; 2.900650]; + + case 101 + % best from 0005 + % peak torque 39.422 Nm ! (from 0005, #4475) + % design rate = .85 m/s + % actual rate fd = .85021 m/s + % dtheta_fixed_fd = -1.4430 + % dtheta_fixed_zd = -1.4431 + + dtheta_fixed = -1.4430; + + %dtheta = -1.4429; + m = -0.5827; + b = 3.4285; + aa = [-0.145395; -0.263783; -0.368703; + -0.257797; -0.148614; -0.107673; -0.145395]; + bb = [-0.017859; -0.416229; -0.498426; + -0.210251; -0.127241; -0.021835; -0.017859]; + cc = [2.917536; 2.366868; 2.372859; + 2.868797; 2.805203; 2.877889; 2.971748]; + dd = [2.971748; 2.960407; 2.517592; + 2.286506; 2.105212; 2.529779; 2.917536]; + case 102 + % best from 0021 + % peak torque 42.23 Nm ! (from 0020, #2035) + % design rate = .95 m/s + % actual rate fd = .9497 m/s + % dtheta_fixed_fd = -1.48705 + % dtheta_fixed_zd = -1.48705 + + dtheta_fixed = -1.48705; + + %dtheta = -1.4873; + m = -0.5772; + b = 3.4262; + aa = [-0.179641; -0.287638; -0.335568; + -0.441010; -0.149923; -0.140826; -0.179641]; + bb = [-0.015911; -0.414943; -0.796324; + -0.244587; -0.280635; 0.062087; -0.015911]; + cc = [2.899138; 2.276354; 2.378248; + 2.586051; 3.015630; 2.895427; 2.941676]; + dd = [2.941676; 2.891617; 2.255246; + 2.161191; 1.989449; 2.713727; 2.899138]; + + case 100102 % transition from 100 to 102 + m = -0.58111; + b = 3.4301; + + aa = [-0.10423; -0.19935; -0.38046; -0.19649; + -0.13822; -0.14056; -0.17964]; + bb = [-0.023751; -0.40965; -0.79565; -0.6473; + -0.2839; 0.062615; -0.015911]; + cc = [2.9007; 2.574; 2.2241; 2.8952; + 2.7863; 2.8951; 2.9417]; + dd = [2.9701; 3.1051; 2.3122; 2.2062; + 1.6772; 2.7125; 2.8991]; +end +a = [aa; bb; cc; dd]; + +% for calculating initial control sensitivities +if ~isempty(WHICH_TO_TWEAK) + if WHICH_TO_TWEAK ~= 0 + if abs(a(WHICH_TO_TWEAK + 4)) < .01 + a(WHICH_TO_TWEAK + 4) = a(WHICH_TO_TWEAK + 4) + (TWEAK_AMOUNT - 1); + else + a(WHICH_TO_TWEAK + 4) = a(WHICH_TO_TWEAK + 4)*TWEAK_AMOUNT; + end + end +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/control_synth.m b/demos/rabbit/anim/rabbit_model/control_synth.m new file mode 100644 index 0000000..948e53f --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/control_synth.m @@ -0,0 +1,39 @@ +function u = control_synth(x) +%CONTROL_SYNTH Synthesize control associated with the supplied +% state. +% +% U = CONTROL_SYNTH(X) + +%Eric Westervelt +%3/13/01 + +[n,m] = size(x); + +gains = [1, .8, .6, .6]; + +if n ~= 1 & m ~= 1 + u = zeros(n,4); + for k = 1:n + [H,LfH,L2fH,LgLfH] = decouple_abs_red(x(k,1:5)',x(k,6:10)'); + + out = [bb([H(1),LfH(1)],gains(1)); + bb([H(2),LfH(2)],gains(2)); + bb([H(3),LfH(3)],gains(3)); + bb([H(4),LfH(4)],gains(4))]; + + % Used for controller that use feedback linearization + u(k,:) = (inv(LgLfH)*(out(:,1)-L2fH))'; + end +else + if n < m, x = x'; end % transpose if necessary + + [H,LfH,L2fH,LgLfH] = decouple_abs_red(x(1:5),x(6:10)); + + out = [bb([H(1),LfH(1)],gains(1)); + bb([H(2),LfH(2)],gains(2)); + bb([H(3),LfH(3)],gains(3)); + bb([H(4),LfH(4)],gains(4))]; + + % Used for controller that use feedback linearization + u = (inv(LgLfH)*(out(:,1)-L2fH))'; +end diff --git a/demos/rabbit/anim/rabbit_model/dyn_mod_abs.m b/demos/rabbit/anim/rabbit_model/dyn_mod_abs.m new file mode 100644 index 0000000..90cfe68 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/dyn_mod_abs.m @@ -0,0 +1,97 @@ +function [D,C,G,B,E]=dyn_mod_abs(x) +% DYN_MOD_ABS Grizzle's full model of kneed biped walker model. +% [DE,CE,GE,BE,EE] = DYN_MOD_ABS(X) is the kneed +% biped walking model. (x is of dimension 14) +%Eric Westervelt +%16-Feb-2001 16:13:37 + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]= modelParameters; + +q31=x(1); q32=x(2); q41=x(3); q42=x(4); y=x(5); z=x(6); q1=x(7); +dq31=x(8); dq32=x(9); dq41=x(10); dq42=x(11); dy=x(12); dz=x(13); dq1=x(14); + +% D matrix +D=zeros(7); +D(1,1)=XX3+M4*L3^2; +D(1,3)=MZ4*L3*cos(-q41+q31); +D(1,5)=-cos(q31)*(M4*L3+MZ3); +D(1,6)=-sin(q31)*(M4*L3+MZ3); +D(2,2)=XX3+M4*L3^2; +D(2,4)=MZ4*L3*cos(-q42+q32); +D(2,5)=-cos(q32)*(M4*L3+MZ3); +D(2,6)=-sin(q32)*(M4*L3+MZ3); +D(3,1)=MZ4*L3*cos(-q41+q31); +D(3,3)=XX4; +D(3,5)=-MZ4*cos(q41); +D(3,6)=-MZ4*sin(q41); +D(4,2)=MZ4*L3*cos(-q42+q32); +D(4,4)=XX4; +D(4,5)=-MZ4*cos(q42); +D(4,6)=-MZ4*sin(q42); +D(5,1)=-cos(q31)*(M4*L3+MZ3); +D(5,2)=-cos(q32)*(M4*L3+MZ3); +D(5,3)=-MZ4*cos(q41); +D(5,4)=-MZ4*cos(q42); +D(5,5)=2*M3+2*M4+M1; +D(5,7)=-cos(q1)*MZ1-sin(q1)*MY1; +D(6,1)=-sin(q31)*(M4*L3+MZ3); +D(6,2)=-sin(q32)*(M4*L3+MZ3); +D(6,3)=-MZ4*sin(q41); +D(6,4)=-MZ4*sin(q42); +D(6,6)=2*M3+2*M4+M1; +D(6,7)=-sin(q1)*MZ1+cos(q1)*MY1; +D(7,5)=-cos(q1)*MZ1-sin(q1)*MY1; +D(7,6)=-sin(q1)*MZ1+cos(q1)*MY1; +D(7,7)=XX1; + +% C matrix +C=zeros(7); +C(1,3)=MZ4*L3*sin(-q41+q31)*dq41; +C(2,4)=MZ4*L3*sin(-q42+q32)*dq42; +C(3,1)=-MZ4*L3*sin(-q41+q31)*dq31; +C(4,2)=-MZ4*L3*sin(-q42+q32)*dq32; +C(5,1)=sin(q31)*(M4*L3+MZ3)*dq31; +C(5,2)=sin(q32)*(M4*L3+MZ3)*dq32; +C(5,3)=MZ4*sin(q41)*dq41; +C(5,4)=MZ4*sin(q42)*dq42; +C(5,7)=(sin(q1)*MZ1-cos(q1)*MY1)*dq1; +C(6,1)=-cos(q31)*(M4*L3+MZ3)*dq31; +C(6,2)=-cos(q32)*(M4*L3+MZ3)*dq32; +C(6,3)=-MZ4*cos(q41)*dq41; +C(6,4)=-MZ4*cos(q42)*dq42; +C(6,7)=(-cos(q1)*MZ1-sin(q1)*MY1)*dq1; + +% G matrix +G=zeros(7,1); +G(1)=-g*sin(q31)*(MZ3+L3*M4); +G(2)=-g*sin(q32)*(MZ3+L3*M4); +G(3)=-g*MZ4*sin(q41); +G(4)=-g*MZ4*sin(q42); +G(6)=g*(M1+2*M3+2*M4); +G(7)=-g*(sin(q1)*MZ1-cos(q1)*MY1); + +% B matrix +B=zeros(7,4); +B(1,1)=1; +B(1,3)=-1; +B(2,2)=1; +B(2,4)=-1; +B(3,3)=1; +B(4,4)=1; +B(7,1)=-1; +B(7,2)=-1; + +% E matrix +E=zeros(7,4); +E(1,1)=-L3*cos(q31); +E(1,2)=-L3*sin(q31); +E(2,3)=-L3*cos(q32); +E(2,4)=-L3*sin(q32); +E(3,1)=-L4*cos(q41); +E(3,2)=-L4*sin(q41); +E(4,3)=-L4*cos(q42); +E(4,4)=-L4*sin(q42); +E(5,1)=1; +E(5,3)=1; +E(6,2)=1; +E(6,4)=1; diff --git a/demos/rabbit/anim/rabbit_model/energy_compute.m b/demos/rabbit/anim/rabbit_model/energy_compute.m new file mode 100644 index 0000000..4efe945 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/energy_compute.m @@ -0,0 +1,9 @@ +function energy_compute(t,u) +nt=length(t); E=0; + for k=1:nt + if k>1 + dt=t(k)-t(k-1); + E=u(k,:)*u(k,:)'*dt+E; + end + end +display(E,'Energy Consumption'); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/fixlength.m b/demos/rabbit/anim/rabbit_model/fixlength.m new file mode 100644 index 0000000..47e7932 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/fixlength.m @@ -0,0 +1,39 @@ +function K = fixlength(s1,s2,len,indent) +%FIXLENGTH Returns a string which has been divided up into < LEN +%character chuncks with the '...' line divide appended at the end +%of each chunck. +% FIXLENGTH(S1,S2,L) is string S1 with string S2 used as break points into +% chuncks less than length L. + +%Eric Westervelt +%5/31/00 +%1/11/01 - updated to use more than one dividing string +%4/29/01 - updated to allow for an indent + +tmp=s1; +K=[]; +count=0; +while length(tmp) > len, + I = []; + for c = 1:length(s2) + I = [I findstr(tmp,s2(c))]; + I = sort(I); + end + if isempty(I) & count == 0 + K = []; + error('S2 does not exist in S1') + end + II = find(I <= len); + if isempty(II) + K = []; + error('Cannot fixlength of S1 on basis of S2') + end + if nargin > 3 + K = [K,tmp(1:I(II(length(II)))),' ...',10,indent]; + else + K = [K,tmp(1:I(II(length(II)))),' ...',10]; + end + tmp = tmp(I(II(length(II)))+1:length(tmp)); + count = count+1; +end +K = [K,tmp]; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/gen_zd.m b/demos/rabbit/anim/rabbit_model/gen_zd.m new file mode 100644 index 0000000..ddbb986 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/gen_zd.m @@ -0,0 +1,370 @@ +function gen_zd +%GEN_ZD Derive the zero dynamics for the kneed biped walker with +% Franck's offset masses model using Bezier functions. + +%Eric Westervelt +%1/8/01 + +if isempty(dir('mat_files/work_symb_zero_dynamics.mat')) + load mat_files/work_symb_control_abs_red_test + model_data = load('mat_files/work_symb_model_abs_red'); + load mat_files/work_symb_model_abs_red + + % First step is to do a partial feedback linearization of the + % dynamics. This will simplify some of the computations and, + % hopefully, improve the numerical conditioning. See Reyhanoglu, + % McClamroch and van der Schaft, IEEE T-AC, Sept. 1999, pp. 1663. + + % First, change coordinates into all relative plus one absolute + syms q1_rel q2_rel q3_rel q4_rel q5_rel + syms dq1_rel dq2_rel dq3_rel dq4_rel dq5_rel + q_rel = [q1_rel; q2_rel; q3_rel; q4_rel; q5_rel]; + dq_rel = [dq1_rel; dq2_rel; dq3_rel; dq4_rel; dq5_rel]; + + A = [ 1 0 0 0 -1; + 0 1 0 0 -1; + -1 0 1 0 0; + 0 -1 0 1 0; + 0 0 0 0 1]; + % old absolute coordinates in terms of relative coords + q_b = inv(A)*q_rel; + dq_b = inv(A)*dq_rel; + +% D = replace_constants(D); +% C = replace_constants(C); +% G = replace_constants(G); + +% A_inv_trans = inv(A)'; +% +% D_rel = subs(A_inv_trans*D, ... +% {q31,q32,q41,q42,q1}, ... +% {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5)},0); +% C_rel = subs(A_inv_trans*C, ... +% {q31,q32,q41,q42,q1,dq31,dq32,dq41,dq42,dq1}, ... +% {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5), ... +% dq_b(1),dq_b(2),dq_b(3),dq_b(4),dq_b(5)},0); +% G_rel = subs(A_inv_trans*G, ... +% {q31,q32,q41,q42,q1}, ... +% {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5)},0); +% B_rel = A_inv_trans*B; + + PE = replace_constants(model_data.PE); + KE = replace_constants(KE); + + PE_rel = subs(PE, ... + {q31,q32,q41,q42,q1}, ... + {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5)},0); + + KE_rel = subs(KE, ... + {q31,q32,q41,q42,q1,dq31,dq32,dq41,dq42,dq1}, ... + {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5), ... + dq_b(1),dq_b(2),dq_b(3),dq_b(4),dq_b(5)},0); + + G_rel = jacobian(PE_rel,q_rel).'; + G_rel = simple(G_rel); + D_rel = jacobian(KE_rel,dq_rel).'; + D_rel = jacobian(D_rel,dq_rel); + + syms C real + n=max(size(q_rel)); + for k=1:n + for j=1:n + C_rel(k,j)=0*g; + for i=1:n + C_rel(k,j)=C_rel(k,j)+1/2*(diff(D_rel(k,j),q_rel(i)) + ... + diff(D_rel(k,i),q_rel(j)) - ... + diff(D_rel(i,j),q_rel(k)))*dq_rel(i); + end + end + end + C_rel = C_rel; + + Phi_0_rel = subs(Phi_0, ... + {q31,q32,q41,q42,q1}, ... + {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5)},0); + + Phi_0_rel = subs(Phi_0, ... + {q31,q32,q41,q42,q1}, ... + {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5)},0); + B_rel = jacobian(Phi_0_rel,q_rel); + B_rel = B_rel'*[eye(4);zeros(1,4)]; + + [n,m] = size(B_rel); + F = -C_rel*dq_rel-G_rel; + D21_rel = D_rel((m+1):n,1:m); + D22_rel = D_rel((m+1):n,(m+1):n); + F1 = F(1:m); + F2 = F((m+1):n); + + % M_bar = simple(M11-M12*inv(M22)*M21); + % F_bar = simple(F1-M12*inv(M22)*F2); + % B_bar = N*B; + % B_bar = B_bar(1:m,:); + % J = simple(-inv(M22)*M21); + % R = simple(-inv(M22)*F2); % u=inv(B_bar)*[M_bar*v + F_bar] + % F_new = [dq;zeros(4,1);R]; + % G_new = [zeros(5,4);eye(4);J]; + + %needed to complete coordinate change...solution is a consequence + %of Frobenius' Theorem...see below + % + D_bar = [eye(4) zeros(4,1); D21_rel D22_rel]; + CG_bar = [eye(4,1); F2]; + + % the output function and its time derivative + H_rel = subs(h, ... + {q31,q32,q41,q42,q1}, ... + {q_b(1),q_b(2),q_b(3),q_b(4),q_b(5)},0); + LfH_rel = jacobian(H_rel,q_rel)*dq_rel; + + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Change of coordinates -- attempting to complete with a result of + % Frobenius's Theorem + % + xh = [H_rel;LfH_rel;(q_b(1)+q_b(3))/2]; + + % compute augmented B matrix + B_aug = [B_rel,[zeros(4,1);1]]; + + % compute last entry of change of coordinates as last row of ... + tmp = inv(B_aug)*D_bar*dq_rel; + xh = [xh; tmp(5)]; + + tmp_abs = inv([B [zeros(4,1);1]])*D*dq; + gamma_abs = replace_constants(tmp_abs(5)); + + xh = replace_constants(xh); + + xh = xh; + disp('[Computed change of coordinates]'); + + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Invert map + % + syms dz1 dz2 z1 z2 real + + syms P1 P2 P3 P4 real % other terms are zero + P_tmp = [P1; P2; P3; P4]; + + Ah_rel = Ah*inv(A); + + q_rel_s_zd = inv(Ah_rel)*[P_tmp;z1]; + + q_rel_s_zd = simple(q_rel_s_zd); + + disp(['[computed position coordinates restricted to zero dynamics' ... + ' manifold]']) + + syms jac_P1 jac_P2 jac_P3 jac_P4 real + temp_1 = [jac_P1; jac_P2; jac_P3; jac_P4]; + theta = (q_b(1)+q_b(3))/2; + temp_1 = [-temp_1*jacobian(theta,q_rel); ... + D21_rel D22_rel]; + + disp(['[computed parts needed to compute velocity coordinates' ... + ' restricted to zero dynamics manifold]']); + + dq_rel_s_zd_almost = inv([Ah_rel(1:4,:);zeros(1,5)] + temp_1)* ... + [zeros(4,1); z2]; + + dq_rel_s_zd = subs(dq_rel_s_zd_almost, q1_rel, q_rel_s_zd(1),0); + dq_rel_s_zd = subs(dq_rel_s_zd, q2_rel, q_rel_s_zd(2),0); + dq_rel_s_zd = subs(dq_rel_s_zd, q3_rel, q_rel_s_zd(3),0); + dq_rel_s_zd = subs(dq_rel_s_zd, q4_rel, q_rel_s_zd(4),0); + dq_rel_s_zd = subs(dq_rel_s_zd, q5_rel, q_rel_s_zd(5),0); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Take time derivative in new coordinates...the system with the + % change of coordinates and decoupling feedback (minus the last + % two states)...we know this is the form because this is what + % feedback linearization does!... + % + %syms v1 v2 v3 v4 real % inputs + %dxh = [xh_5; xh_6; xh_7; xh_8; v1; v2; v3; v4]; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Now for the zero dynamics -- assume all outputs are zero + % + disp('[Calculating zero dynamics...]'); + + dz1 = simple(jacobian(theta,q_rel)*dq_rel_s_zd) + F2 = replace_constants(F2); + dz2 = -G_rel(n); + + % now dxh has all its entries + %dxh_zd = [dxh_zd, dz2]; + + dz2 = subs(dz2,q1_rel,q_rel_s_zd(1),0); + dz2 = subs(dz2,q2_rel,q_rel_s_zd(2),0); + dz2 = subs(dz2,q3_rel,q_rel_s_zd(3),0); + dz2 = subs(dz2,q4_rel,q_rel_s_zd(4),0); + dz2 = subs(dz2,q5_rel,q_rel_s_zd(5),0); + dz2 = simple(dz2) + disp('[The zero dynamics in z1, z2 coordinates !]'); + + save mat_files/work_symb_zero_dynamics + disp('[save mat_files/work_symb_zero_dynamics]'); +else + load mat_files/work_symb_zero_dynamics + disp('[DONE loading work_symb_zero_dynamics.mat]') +end + +file_name = 'dz1'; +disp(['[creating ',upper(file_name),'.txt]']); +fid = fopen(['doc/',file_name,'.txt'],'w'); +[r,s] = subexpr(dz1,'s'); +fprintf(fid,'%s\n',num2str(length(s))); +fprintf(fid,'%s\n',latex(r)); +for k = 1:length(s) + fprintf(fid,'s(%s)&=&%s\n',num2str(k),latex(s(k))); +end +fclose(fid); + +file_name = 'dz2'; +disp(['[creating ',upper(file_name),'.txt]']); +fid = fopen(['doc/',file_name,'.txt'],'w'); +fprintf(fid,'0\n'); +fprintf(fid,'%s\n',latex(dz2)); +fclose(fid); + +fcn_name = 'zd_project'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +fprintf(fid,'function z =%s(x)\n',fcn_name); +fprintf(fid,['%%%s Projects the zero dynamics to the zero' ... + ' dynamics.\n'],upper(fcn_name)); +fprintf(fid,'%%\n%% Z = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +% Reassign configuration parameters +fprintf(fid,'q31=x(1); q32=x(2); q41=x(3); q42=x(4); q1=x(5);\n'); +fprintf(fid,'dq31=x(6); dq32=x(7); dq41=x(8); dq42=x(9); dq1=x(10);\n\n'); + +fprintf(fid,'%% theta (in absolute coordinates)\n'); +fprintf(fid,'z(1) = (q31+q41)/2;\n\n'); + +fprintf(fid,'%% gamma (in absolute coordinates)\n'); +fprintf(fid,'z(2) = %s;',char(gamma_abs)); +fclose(fid); + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Automatically generate .m-files needed for simulation +% +fcn_name = 'zd'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +header=['function dz = ',fcn_name,'(z)']; +fprintf(fid,'%s\n',header); +fprintf(fid,'%%%s\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[n,m] = size(z);\n'); +fprintf(fid,'if n == 1 | m == 1\n'); +fprintf(fid,' z1 = z(1); z2 = z(2);\n'); +fprintf(fid,'else\n'); +fprintf(fid,' z1 = z(:,1); z2 = z(:,2);\n'); +fprintf(fid,'end\n\n'); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +a = char('a'); +for j = 0:3 + for k = 0:NN + fprintf(fid,[char(a+j),num2str(k),'=a(',num2str(1+k+j*(NN+1)),'); ']); + end + fprintf(fid,'\n'); +end +fprintf(fid,'\n'); + +jac_P = jacobian(P,theta1); +jac_P = replace_constants(jac_P); +jac_P = subs(jac_P,theta1,z1); +P = replace_constants(P); +P = subs(P,theta1,z1); + +ttt = vectorize(char(P(1))); +fprintf(fid,'P1 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(P(2))); +fprintf(fid,'P2 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(P(3))); +fprintf(fid,'P3 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(P(4))); +fprintf(fid,'P4 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(jac_P(1))); +fprintf(fid,'jac_P1 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(jac_P(2))); +fprintf(fid,'jac_P2 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(jac_P(3))); +fprintf(fid,'jac_P3 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(jac_P(4))); +fprintf(fid,'jac_P4 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + + +% State one +% +clear r s +[r,s] = subexpr(dz1,'s'); +if length(s) == 0 +elseif length(s) > 1 + for k = 1:length(s) + ttt = char(vectorize(s(k))); + fprintf(fid,'s(%s) = %s;\n',num2str(k), ... + fixlength(ttt,'*+-',65,' ')); + end +else + ttt = char(vectorize(s)); + fprintf(fid,'s = %s;\n',fixlength(ttt,'*+-',65,' ')); +end +fprintf(fid,'\n'); + +ttt = char(vectorize(r)); +fprintf(fid,'%% dz1\n'); +fprintf(fid,'dz_1 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + + +% State two +% +ttt = char(vectorize(dz2)); +fprintf(fid,'%% dz2\n'); +fprintf(fid,'dz_2 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +fprintf(fid,'[n,m] = size(z);\n'); +fprintf(fid,'if n == 1 | m == 1\n'); +fprintf(fid,' dz = [dz_1; dz_2];\n'); +fprintf(fid,'else\n'); +fprintf(fid,' dz = [dz_1, dz_2];\n'); +fprintf(fid,'end'); +fclose(fid); + + +% ------------------------------------------------------ + +function y = replace_constants(x) + +syms g b m +syms L1 L3 L4 M1 M3 M4 MY1 MZ1 MZ3 MZ4 XX1 XX3 XX4 + +[a,b_,m_] = controlParameters; + +[g_,L1_,L3_,L4_,M1_,M3_,M4_,MY1_,MZ1_,MZ3_,MZ4_,XX1_,XX3_,XX4_] = ... + modelParameters; + +y = subs(x,{g, ... + L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4}, ... + {g_, ... + L1_,L3_,L4_,M1_,M3_,M4_,MY1_,MZ1_,MZ3_,MZ4_,XX1_, ... + XX3_,XX4_},0); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/gen_zd_cc.m b/demos/rabbit/anim/rabbit_model/gen_zd_cc.m new file mode 100644 index 0000000..01c3631 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/gen_zd_cc.m @@ -0,0 +1,168 @@ +function gen_zd_cc +%GEN_ZD_CC Derive a change of coordinates for the zero dynamics +% to go from [theta_1 gamma] coordinates to [theta; dtheta] +% coordinates. + +%Eric Westervelt +%2/15/01 + +clear * + +if isempty(dir('mat_files/work_symb_zero_dynamics.mat')) + error('WORK_SYMB_ZERO_DYNAMICS.MAT not found...run GEN_ZD.M first'); +else + load mat_files/work_symb_zero_dynamics +end + +fcn_name = 'zd_project_cc'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +fprintf(fid,'function z = %s(x)\n',fcn_name); +fprintf(fid,['%%%s Projects the zero dynamics to the zero' ... + ' dynamics.\n'],upper(fcn_name)); +fprintf(fid,'%%\n'); +fprintf(fid,'%% Z = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'%% first project\n'); +fprintf(fid,'z = zd_project(x);\n'); +fprintf(fid,'dz = zd(z);\n'); +fprintf(fid,'x_zd = zd_lift(z(1),dz(1));\n\n'); + +fprintf(fid,'%% theta\n'); +fprintf(fid,'z(1) = 1/2*(x_zd(1)+x_zd(3));\n\n'); + +fprintf(fid,'%% dtheta\n'); +fprintf(fid,'z(2) = 1/2*(x_zd(6)+x_zd(8));'); +fclose(fid); + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Automatically generate .m-file needed for simulation +% +fcn_name = 'zd_cc'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +fprintf(fid,'function dz = %s(z)\n',fcn_name); +fprintf(fid,'%%%s\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'z1 = z(1); z2 = z(2);\n\n'); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +fprintf(fid,['a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9);' ... + ' a5=a(10); a6=a(11);\n']); +fprintf(fid,['b0=a(12); b1=a(13); b2=a(14); b3=a(15); b4=a(16);' ... + ' b5=a(17); b6=a(18);\n']); +fprintf(fid,['c0=a(19); c1=a(20); c2=a(21); c3=a(22); c4=a(23);' ... + ' c5=a(24); c6=a(25);\n']); +fprintf(fid,['d0=a(26); d1=a(27); d2=a(28); d3=a(29); d4=a(30);' ... + ' d5=a(31); d6=a(32);\n\n']); + +jac_Ph = jacobian(Ph,theta1); +jac_Ph = replace_constants(jac_Ph); +jac_Ph = subs(jac_Ph,theta1,z1); +Ph = replace_constants(Ph); +Ph = subs(Ph,theta1,z1); + +ttt = char(Ph(1)); +fprintf(fid,'Ph1 = %s;\n\n',fixlength(ttt,'*',75)); + +ttt = char(Ph(2)); +fprintf(fid,'Ph2 = %s;\n\n',fixlength(ttt,'*',75)); + +ttt = char(Ph(3)); +fprintf(fid,'Ph3 = %s;\n\n',fixlength(ttt,'*',75)); + +ttt = char(Ph(4)); +fprintf(fid,'Ph4 = %s;\n\n',fixlength(ttt,'*',75)); + + +% In new coordinates!! theta1, dtheta1; but still need beta (was +% called dz_2) for calculation of dz_2 in new coordinates...see +% below +% +% (see notes 1/22/01 for calculations) +% +% stuff needed to calculate zerod dynamics in coordinates of +% +% z = [theta; dtheta] +% +disp('[calculating the new coordinates]'); +alpha = dz1/z2; +alpha_subs = subs(alpha, ... + {Ph1,Ph2,Ph3,Ph4,jac_Ph1,jac_Ph2,jac_Ph3,jac_Ph4}, ... + {Ph(1),Ph(2),Ph(3),Ph(4),jac_Ph(1),jac_Ph(2), ... + jac_Ph(3),jac_Ph(4)},0); +alpha_subs = subs(alpha_subs,theta1,z1,0); +jac_alpha_subs = jacobian(alpha_subs,z1); +save mat_files/work_symb_zero_dynamics_cc +disp('[save mat_files/work_symb_zero_dynamics_cc]'); + +[r_alpha_subs,s_alpha_subs] = subexpr(alpha_subs,'s'); +if length(s_alpha_subs) == 0 +elseif length(s_alpha_subs) > 1 + for k = 1:length(s_alpha_subs) + ttt = char(s_alpha_subs(k)); + fprintf(fid,'s(%s) = %s;\n',num2str(k),fixlength(ttt,'*',75)); + end +else + ttt = char(s_alpha_subs); + fprintf(fid,'s = %s;\n',fixlength(ttt,'*',75)); +end +fprintf(fid,'\n'); +ttt = char(r_alpha_subs); +fprintf(fid,'%% alpha\n'); +ttt_fl = fixlength(ttt,'*',75); +fprintf(fid,'alpha = %s;\n\n',ttt_fl); + +[r_jac_alpha_subs,s_jac_alpha_subs] = subexpr(jac_alpha_subs,'s'); +if length(s_jac_alpha_subs) == 0 +elseif length(s_jac_alpha_subs) > 1 + for k = 1:length(s_jac_alpha_subs) + ttt = char(s_jac_alpha_subs(k)); + fprintf(fid,'s(%s) = %s;\n',num2str(k),fixlength(ttt,'*',75)); + end +else + ttt = char(s_jac_alpha_subs); + fprintf(fid,'s = %s;\n',fixlength(ttt,'*',75)); +end +fprintf(fid,'\n'); +ttt = char(r_jac_alpha_subs); +ttt_fl = fixlength(ttt,'*',75); +fprintf(fid,'jac_alpha = %s;\n\n',ttt_fl); + +fprintf(fid,'%% change to original z coordinates\n'); +fprintf(fid,'%%\n'); +fprintf(fid,'%% NOTE! z1 (in old coords) = z1 (in new coords)\n'); +fprintf(fid,'z_old = [z1; 1/alpha*z2];\n\n'); +fprintf(fid,'dz_old = zd(z_old);\n\n'); +fprintf(fid,'dz = [z2; jac_alpha/alpha*z2^2+alpha*dz_old(2)];'); +fclose(fid); + + +% ------------------------------------------------------ + +function y = replace_constants(x) + +syms ah1 ah2 ah3 ah4 +syms g b m +syms L1 L3 L4 M1 M3 M4 MY1 MZ1 MZ3 MZ4 XX1 XX3 XX4 + +[a,b_,m_] = controlParameters; + +[g_,L1_,L3_,L4_,M1_,M3_,M4_,MY1_,MZ1_,MZ3_,MZ4_,XX1_,XX3_,XX4_] = ... + modelParameters; + +ah1_ = a(1); ah2_ = a(2); ah3_ = a(3); ah4_ = a(4); + +y = subs(x,{g,ah1,ah2,ah3,ah4,b,m,... + L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4}, ... + {g_,ah1_,ah2_,ah3_,ah4_,b_,m_ ... + L1_,L3_,L4_,M1_,M3_,M4_,MY1_,MZ1_,MZ3_,MZ4_,XX1_, ... + XX3_,XX4_},0); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/gen_zd_diff.m b/demos/rabbit/anim/rabbit_model/gen_zd_diff.m new file mode 100644 index 0000000..d4cb1b7 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/gen_zd_diff.m @@ -0,0 +1,237 @@ +function gen_zd_diff +%GEN_ZD_DIFF Derive the first order sensitivies of the zero +% dynamics to the control parameters a0,...,a5, b0,...b5, and +% c0,...,c5 for the kneed biped walker with Franck's offset masses +% model using Bezier functions. + +%Eric Westervelt +%1/10/01 + +clear * + +if isempty(dir('mat_files/zd_work5.mat')) + error('ZD_WORK5.MAT not found...run GEN_ZD.M first'); +else + load mat_files/zd_work5 +end + +for k = 1:3 + disp(' '); + disp(['[WARNING!! This script can take over 30 minutes to' ... + ' run.]']); +end +disp(' '); + +if 1 + jac_Ph = jacobian(Ph,theta1); + jac_Ph = replace_constants(jac_Ph); + jac_Ph = subs(jac_Ph,theta1,z1); + Ph = replace_constants(Ph); + Ph = subs(Ph,theta1,z1); + + dz1_new = subs(dz1,Ph1,Ph(1)); + dz1_new = subs(dz1_new,Ph2,Ph(2)); + dz1_new = subs(dz1_new,Ph3,Ph(3)); + dz1_new = subs(dz1_new,Ph4,Ph(4)); + + dz1_new = subs(dz1_new,jac_Ph1,jac_Ph(1)); + dz1_new = subs(dz1_new,jac_Ph2,jac_Ph(2)); + dz1_new = subs(dz1_new,jac_Ph3,jac_Ph(3)); + dz1_new = subs(dz1_new,jac_Ph4,jac_Ph(4)); + + dz2_new = subs(dz2,Ph1,Ph(1)); + dz2_new = subs(dz2_new,Ph2,Ph(2)); + dz2_new = subs(dz2_new,Ph3,Ph(3)); + dz2_new = subs(dz2_new,Ph4,Ph(4)); + + save mat_files/zd_work7 + disp('[save mat_files/zd_work7]'); + + syms a0_0 a1_0 a2_0 a3_0 a4_0 a5_0 + syms b0_0 b1_0 b2_0 b3_0 b4_0 b5_0 + syms c0_0 c1_0 c2_0 c3_0 c4_0 c5_0 + syms d0_0 d1_0 d2_0 d3_0 d4_0 d5_0 + + dz_partial_a0 = compute_diff_sens(dz1_new, dz2_new, a0, a0_0); + dz_partial_a1 = compute_diff_sens(dz1_new, dz2_new, a1, a1_0); + dz_partial_a2 = compute_diff_sens(dz1_new, dz2_new, a2, a2_0); + dz_partial_a3 = compute_diff_sens(dz1_new, dz2_new, a3, a3_0); + dz_partial_a4 = compute_diff_sens(dz1_new, dz2_new, a4, a4_0); + dz_partial_a5 = compute_diff_sens(dz1_new, dz2_new, a5, a5_0); + + disp(' ') + + dz_partial_b0 = compute_diff_sens(dz1_new, dz2_new, b0, b0_0); + dz_partial_b1 = compute_diff_sens(dz1_new, dz2_new, b1, b1_0); + dz_partial_b2 = compute_diff_sens(dz1_new, dz2_new, b2, b2_0); + dz_partial_b3 = compute_diff_sens(dz1_new, dz2_new, b3, b3_0); + dz_partial_b4 = compute_diff_sens(dz1_new, dz2_new, b4, b4_0); + dz_partial_b5 = compute_diff_sens(dz1_new, dz2_new, b5, b5_0); + + disp(' ') + + dz_partial_c0 = compute_diff_sens(dz1_new, dz2_new, c0, c0_0); + dz_partial_c1 = compute_diff_sens(dz1_new, dz2_new, c1, c1_0); + dz_partial_c2 = compute_diff_sens(dz1_new, dz2_new, c2, c2_0); + dz_partial_c3 = compute_diff_sens(dz1_new, dz2_new, c3, c3_0); + dz_partial_c4 = compute_diff_sens(dz1_new, dz2_new, c4, c4_0); + dz_partial_c5 = compute_diff_sens(dz1_new, dz2_new, c5, c5_0); + + disp(' ') + + dz_partial_d0 = compute_diff_sens(dz1_new, dz2_new, d0, d0_0); + dz_partial_d1 = compute_diff_sens(dz1_new, dz2_new, d1, d1_0); + dz_partial_d2 = compute_diff_sens(dz1_new, dz2_new, d2, d2_0); + dz_partial_d3 = compute_diff_sens(dz1_new, dz2_new, d3, d3_0); + dz_partial_d4 = compute_diff_sens(dz1_new, dz2_new, d4, d4_0); + dz_partial_d5 = compute_diff_sens(dz1_new, dz2_new, d5, d5_0); + + save mat_files/zd_work8 + disp('[save mat_files/zd_work8]'); +end +load mat_files/zd_work8 +disp('[DONE computing/loading]') + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Automatically generate .m-file needed for simulation +% +fcn_name = 'zd_diff'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +fprintf(fid,'function dz = %s(z,opt)\n',fcn_name); +fprintf(fid,'%%%s\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +error('HERE -- need to fix the parameters...see SYMB_CONTROL.M') + +fprintf(fid,['a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9);' ... + ' a5=a(10);\n']); +fprintf(fid,['b0=a(11); b1=a(12); b2=a(13); b3=a(14); b4=a(15);' ... + ' b5=a(16);\n']); +fprintf(fid,['c0=a(17); c1=a(18); c2=a(19); c3=a(20); c4=a(21);' ... + ' c5=a(22);\n']); +fprintf(fid,['d0=a(23); d1=a(24); d2=a(25); d3=a(26); d4=a(27);' ... + ' d5=a(28);\n\n']); + +fprintf(fid,['a0_0=a(5); a1_0=a(6); a2_0=a(7); a3_0=a(8); a4_0=a(9);' ... + ' a5_0=a(10);\n']); +fprintf(fid,['b0_0=a(11); b1_0=a(12); b2_0=a(13); b3_0=a(14); b4_0=a(15);' ... + ' b5_0=a(16);\n']); +fprintf(fid,['c0_0=a(17); c1_0=a(18); c2_0=a(19); c3_0=a(20); c4_0=a(21);' ... + ' c5_0=a(22);\n']); +fprintf(fid,['d0_0=a(23); d1_0=a(24); d2_0=a(25); d3_0=a(26); d4_0=a(27);' ... + ' d5_0=a(28);\n\n']); + +fprintf(fid,'z1 = z(1); z2 = z(2);\n\n'); + +fprintf(fid,'switch opt\n'); + +num = 1; + +num = print_diff_sens(fid,dz_partial_a0, num); +num = print_diff_sens(fid,dz_partial_a1, num); +num = print_diff_sens(fid,dz_partial_a2, num); +num = print_diff_sens(fid,dz_partial_a3, num); +num = print_diff_sens(fid,dz_partial_a4, num); +num = print_diff_sens(fid,dz_partial_a5, num); + +num = print_diff_sens(fid,dz_partial_b0, num); +num = print_diff_sens(fid,dz_partial_b1, num); +num = print_diff_sens(fid,dz_partial_b2, num); +num = print_diff_sens(fid,dz_partial_b3, num); +num = print_diff_sens(fid,dz_partial_b4, num); +num = print_diff_sens(fid,dz_partial_b5, num); + +num = print_diff_sens(fid,dz_partial_c0, num); +num = print_diff_sens(fid,dz_partial_c1, num); +num = print_diff_sens(fid,dz_partial_c2, num); +num = print_diff_sens(fid,dz_partial_c3, num); +num = print_diff_sens(fid,dz_partial_c4, num); +num = print_diff_sens(fid,dz_partial_c5, num); + +num = print_diff_sens(fid,dz_partial_d0, num); +num = print_diff_sens(fid,dz_partial_d1, num); +num = print_diff_sens(fid,dz_partial_d2, num); +num = print_diff_sens(fid,dz_partial_d3, num); +num = print_diff_sens(fid,dz_partial_d4, num); +num = print_diff_sens(fid,dz_partial_d5, num); + +fprintf(fid,' otherwise\n'); +fprintf(fid,' error(''opt parameter out of range'');\n'); +fprintf(fid,'end\n'); +fclose(fid); + + +% ------------------------------------------------------ + +function dz_partial_xx = compute_diff_sens(dz1, dz2, xx, xx_0) + +xx_sym = sym(inputname(3)); + +% State one +dz_partial_xx(1) = jacobian(dz1,xx_sym); +dz_partial_xx(1) = subs(dz_partial_xx(1),xx_sym,xx_0,0); +disp(['[computed partial dz1/partial ',inputname(3),']']) + +% State two +dz_partial_xx(2) = jacobian(dz2,xx_sym); +dz_partial_xx(2) = subs(dz_partial_xx(2),xx_sym,xx_0,0); +disp(['[computed partial dz2/partial ',inputname(3),']']) + + +% ------------------------------------------------------ + +function new_num = print_diff_sens(fid, dz_partial_xx, num) + +new_num = num + 1; + +fprintf(fid,' case %s,\n', num2str(num)); + +% State one +fprintf(fid,' %% %s (state one)\n',inputname(2)); +clear r s +[r,s] = subexpr(dz_partial_xx(1),'s'); +for k = 1:length(s) + fprintf(fid,' s(%s) = %s;\n',num2str(k), ... + fixlength(char(vpa(s(k),10)),'*+^',65)); +end +fprintf(fid,'\n'); +fprintf(fid,' dz(1) = %s;\n\n', ... + num2str(num), ... + fixlength(char(vpa(r,10)),'*+^',65)); + +% State two +fprintf(fid,'\n'); +fprintf(fid,' %% %s (state two)\n',inputname(2)); +fprintf(fid,' dz(2) = %s;\n\n', ... + num2str(num), ... + fixlength(char(vpa(dz_partial_xx(2),10)),'*+^',65)); + +disp(['[printed ',inputname(2),']']); + + +% ------------------------------------------------------ + +function y = replace_constants(x) + +syms ah1 ah2 ah3 ah4 +syms g b m +syms L1 L3 L4 M1 M3 M4 MY1 MZ1 MZ3 MZ4 XX1 XX3 XX4 + +[a,b_,m_] = controlParameters; + +[g_,L1_,L3_,L4_,M1_,M3_,M4_,MY1_,MZ1_,MZ3_,MZ4_,XX1_,XX3_,XX4_] = ... + modelParameters; + +ah1_ = a(1); ah2_ = a(2); ah3_ = a(3); ah4_ = a(4); + +y = subs(x,{g,ah1,ah2,ah3,ah4,b,m,... + L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4}, ... + {g_,ah1_,ah2_,ah3_,ah4_,b_,m_ ... + L1_,L3_,L4_,M1_,M3_,M4_,MY1_,MZ1_,MZ3_,MZ4_,XX1_, ... + XX3_,XX4_},0); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/gen_zd_diff_cc.m b/demos/rabbit/anim/rabbit_model/gen_zd_diff_cc.m new file mode 100644 index 0000000..92e3cdc --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/gen_zd_diff_cc.m @@ -0,0 +1,360 @@ +function gen_zd_diff_cc +%GEN_ZD_CC Derive a change of coordinates for the zero dynamics +% to go from [theta_1 gamma] coordinates to [theta; dtheta] +% coordinates. + +%Eric Westervelt +%2/15/01 + +clear * + +global ALPHA_SUBS JAC_ALPHA_SUBS BETA_SUBS + +if isempty(dir('mat_files/zd_work5.mat')) + error('ZD_WORK5.MAT not found...run GEN_ZD.M first'); +else + load mat_files/zd_work5 +end + +if 0 + if 1 + syms a0_0 a1_0 a2_0 a3_0 a4_0 a5_0 + syms b0_0 b1_0 b2_0 b3_0 b4_0 b5_0 + syms c0_0 c1_0 c2_0 c3_0 c4_0 c5_0 + syms d0_0 d1_0 d2_0 d3_0 d4_0 d5_0 + + jac_Ph = jacobian(Ph,theta1); + jac_Ph = replace_constants(jac_Ph); + jac_Ph = subs(jac_Ph,theta1,z1); + Ph = subs(Ph,theta1,z1); + Ph = replace_constants(Ph); + + % In new coordinates!! theta1, dtheta1; but still need beta (was + % called dz_2) for calculation of dz_2 in new coordinates...see below + % + % (see notes 2/26/01 for calculations) + % + % stuff needed to calculate zerod dynamics in coordinates of + % + % z = [theta; dtheta] + % + disp('[calculating the new coordinates]'); + alpha = dz1/z2; + ALPHA_SUBS = subs(alpha, ... + {Ph1,Ph2,Ph3,Ph4,jac_Ph1,jac_Ph2,jac_Ph3,jac_Ph4}, ... + {Ph(1),Ph(2),Ph(3),Ph(4),jac_Ph(1),jac_Ph(2), ... + jac_Ph(3),jac_Ph(4)},0); + ALPHA_SUBS = subs(ALPHA_SUBS,theta1,z1,0); + ALPHA_SUBS = replace_constants(ALPHA_SUBS); + JAC_ALPHA_SUBS = jacobian(ALPHA_SUBS,z1); + BETA_SUBS = subs(dz2,Ph1,Ph(1)); + BETA_SUBS = subs(BETA_SUBS,Ph2,Ph(2)); + BETA_SUBS = subs(BETA_SUBS,Ph3,Ph(3)); + BETA_SUBS = subs(BETA_SUBS,Ph4,Ph(4)); + BETA_SUBS = replace_constants(BETA_SUBS); + save mat_files/zd_workA + disp('[save mat_files/zd_workA]'); + end + load mat_files/zd_workA + + partial_a0 = compute_diff_sens(a0, a0_0); + partial_a1 = compute_diff_sens(a1, a1_0); + partial_a2 = compute_diff_sens(a2, a2_0); + partial_a3 = compute_diff_sens(a3, a3_0); + partial_a4 = compute_diff_sens(a4, a4_0); + partial_a5 = compute_diff_sens(a5, a5_0); + + disp(' ') + + partial_b0 = compute_diff_sens(b0, b0_0); + partial_b1 = compute_diff_sens(b1, b1_0); + partial_b2 = compute_diff_sens(b2, b2_0); + partial_b3 = compute_diff_sens(b3, b3_0); + partial_b4 = compute_diff_sens(b4, b4_0); + partial_b5 = compute_diff_sens(b5, b5_0); + + disp(' ') + + partial_c0 = compute_diff_sens(c0, c0_0); + partial_c1 = compute_diff_sens(c1, c1_0); + partial_c2 = compute_diff_sens(c2, c2_0); + partial_c3 = compute_diff_sens(c3, c3_0); + partial_c4 = compute_diff_sens(c4, c4_0); + partial_c5 = compute_diff_sens(c5, c5_0); + + disp(' ') + + partial_d0 = compute_diff_sens(d0, d0_0); + partial_d1 = compute_diff_sens(d1, d1_0); + partial_d2 = compute_diff_sens(d2, d2_0); + partial_d3 = compute_diff_sens(d3, d3_0); + partial_d4 = compute_diff_sens(d4, d4_0); + partial_d5 = compute_diff_sens(d5, d5_0); + + save mat_files/zd_workB + disp('[save mat_files/zd_workB]'); +end +load mat_files/zd_workB +disp('[DONE computing/loading]') + + +if 0 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Automatically generate .m-file needed for simulation +% +fcn_name = 'zd_diff_cc'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +fprintf(fid,'function dz = %s(z,opt)\n',fcn_name); +fprintf(fid,'%%%s\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +fprintf(fid,['a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9);' ... + ' a5=a(10);\n']); +fprintf(fid,['b0=a(11); b1=a(12); b2=a(13); b3=a(14); b4=a(15);' ... + ' b5=a(16);\n']); +fprintf(fid,['c0=a(17); c1=a(18); c2=a(19); c3=a(20); c4=a(21);' ... + ' c5=a(22);\n']); +fprintf(fid,['d0=a(23); d1=a(24); d2=a(25); d3=a(26); d4=a(27);' ... + ' d5=a(28);\n\n']); + +fprintf(fid,['a0_0=a(5); a1_0=a(6); a2_0=a(7); a3_0=a(8); a4_0=a(9);' ... + ' a5_0=a(10);\n']); +fprintf(fid,['b0_0=a(11); b1_0=a(12); b2_0=a(13); b3_0=a(14); b4_0=a(15);' ... + ' b5_0=a(16);\n']); +fprintf(fid,['c0_0=a(17); c1_0=a(18); c2_0=a(19); c3_0=a(20); c4_0=a(21);' ... + ' c5_0=a(22);\n']); +fprintf(fid,['d0_0=a(23); d1_0=a(24); d2_0=a(25); d3_0=a(26); d4_0=a(27);' ... + ' d5_0=a(28);\n\n']); + +fprintf(fid,'z1 = z(1); z2 = z(2);\n\n'); + +clear r s +[r,s] = subexpr(ALPHA_SUBS,'s'); +if length(s) == 0 +elseif length(s) > 1 + for k = 1:length(s) + ttt = char(s(k)); + fprintf(fid,'s(%s) = %s;\n',num2str(k),fixlength(ttt,'*',75)); + end +else + fprintf(fid,'s = %s;\n',fixlength(char(s),'*',75)); +end +fprintf(fid,'\n%% alpha\n'); +ttt_fl = fixlength(char(r),'*',75); +fprintf(fid,'alpha = %s;\n\n',ttt_fl); + +clear r s +[r,s] = subexpr(BETA_SUBS,'s'); +if length(s) == 0 +elseif length(s) > 1 + for k = 1:length(s) + ttt = char(s(k)); + fprintf(fid,'s(%s) = %s;\n',num2str(k),fixlength(ttt,'*',75)); + end +else + fprintf(fid,'s = %s;\n',fixlength(char(s),'*',75)); +end +fprintf(fid,'\n%% beta\n'); +ttt_fl = fixlength(char(r),'*',75); +fprintf(fid,'beta = %s;\n\n',ttt_fl); + +clear r s +[r,s] = subexpr(JAC_ALPHA_SUBS,'s'); +if length(s) == 0 +elseif length(s) > 1 + for k = 1:length(s) + ttt = char(s(k)); + fprintf(fid,'s(%s) = %s;\n',num2str(k),fixlength(ttt,'*',75)); + end +else + fprintf(fid,'s = %s;\n',fixlength(char(s),'*',75)); +end +ttt = char(r); +ttt_fl = fixlength(ttt,'*',75); +fprintf(fid,'\njac_alpha = %s;\n\n',ttt_fl); + +fprintf(fid,'switch opt\n'); +for k = 1:24 + fprintf(fid,' case %s,\n',num2str(k)); + switch floor((k-1)/6) + case 0, chr = 'a'; + case 1, chr = 'b'; + case 2, chr = 'c'; + case 3, chr = 'd'; + end + fprintf(fid,[' [partial_alpha,partial_beta,partial_jac_alpha] =' ... + ' zd_diff_cc_',chr,'%s(z1);\n'],num2str(mod(k-1,6))); +end +fprintf(fid,' otherwise\n'); +fprintf(fid,' error(''OPT parameter out of range'');\n'); +fprintf(fid,'end\n\n'); + +% State one +fprintf(fid,'%% state one\n'); +fprintf(fid,'dz(1) = 0\n\n');%z2*partial_alpha;\n\n'); + +% State two +fprintf(fid,'%% state two\n'); +%fprintf(fid,'dz(2) = partial_jac_alpha*z2^2*alpha ...\n'); +%fprintf(fid,' + jac_alpha*z2^2*partial_alpha ...\n'); +%fprintf(fid,' + partial_alpha*beta ...\n'); +%fprintf(fid,' + alpha*partial_beta;'); +fprintf(fid,'dz(2) = z2^2/alpha^2*partial_alpha*jac_alpha ...\n'); +fprintf(fid,' + z2^2/alpha*partial_jac_alpha ...\n'); +fprintf(fid,' + partial_alpha*beta ...\n'); +fprintf(fid,' + alpha*partial_beta;'); +fclose(fid); +end +% +% +% +return +% +for k = 1:24 + switch floor((k-1)/6) + case 0, chr = 'a'; + case 1, chr = 'b'; + case 2, chr = 'c'; + case 3, chr = 'd'; + end + fcn_name = ['zd_diff_cc_',chr,num2str(mod(k-1,6))]; + disp(['[creating ',upper(fcn_name),'.m]']); + fid = fopen([fcn_name,'.m'],'w'); + n=max(size(q)); + fprintf(fid,['function [partial_alpha,partial_beta,partial_jac_alpha] =' ... + ' %s(z1)\n'],fcn_name); + fprintf(fid,'%%%s\n\n',upper(fcn_name)); + fprintf(fid,'%%Eric Westervelt\n'); + fprintf(fid,'%%%s\n\n',datestr(now)); + + fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + + fprintf(fid,['a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9);' ... + ' a5=a(10);\n']); + fprintf(fid,['b0=a(11); b1=a(12); b2=a(13); b3=a(14); b4=a(15);' ... + ' b5=a(16);\n']); + fprintf(fid,['c0=a(17); c1=a(18); c2=a(19); c3=a(20); c4=a(21);' ... + ' c5=a(22);\n']); + fprintf(fid,['d0=a(23); d1=a(24); d2=a(25); d3=a(26); d4=a(27);' ... + ' d5=a(28);\n\n']); + + fprintf(fid,['a0_0=a(5); a1_0=a(6); a2_0=a(7); a3_0=a(8); a4_0=a(9);' ... + ' a5_0=a(10);\n']); + fprintf(fid,['b0_0=a(11); b1_0=a(12); b2_0=a(13); b3_0=a(14);' ... + ' b4_0=a(15); b5_0=a(16);\n']); + fprintf(fid,['c0_0=a(17); c1_0=a(18); c2_0=a(19); c3_0=a(20);' ... + ' c4_0=a(21); c5_0=a(22);\n']); + fprintf(fid,['d0_0=a(23); d1_0=a(24); d2_0=a(25); d3_0=a(26);' ... + ' d4_0=a(27); d5_0=a(28);\n\n']); + + switch k + case 1, print_diff_sens(fid,partial_a0); + case 2, print_diff_sens(fid,partial_a1); + case 3, print_diff_sens(fid,partial_a2); + case 4, print_diff_sens(fid,partial_a3); + case 5, print_diff_sens(fid,partial_a4); + case 6, print_diff_sens(fid,partial_a5); + + case 7, print_diff_sens(fid,partial_b0); + case 8, print_diff_sens(fid,partial_b1); + case 9, print_diff_sens(fid,partial_b2); + case 10, print_diff_sens(fid,partial_b3); + case 11, print_diff_sens(fid,partial_b4); + case 12, print_diff_sens(fid,partial_b5); + + case 13, print_diff_sens(fid,partial_c0); + case 14, print_diff_sens(fid,partial_c1); + case 15, print_diff_sens(fid,partial_c2); + case 16, print_diff_sens(fid,partial_c3); + case 17, print_diff_sens(fid,partial_c4); + case 18, print_diff_sens(fid,partial_c5); + + case 19, print_diff_sens(fid,partial_d0); + case 20, print_diff_sens(fid,partial_d1); + case 21, print_diff_sens(fid,partial_d2); + case 22, print_diff_sens(fid,partial_d3); + case 23, print_diff_sens(fid,partial_d4); + case 24, print_diff_sens(fid,partial_d5); + end +end + + +% ------------------------------------------------------ + +function partial_xx = compute_diff_sens(xx, xx_0) + +global ALPHA_SUBS JAC_ALPHA_SUBS BETA_SUBS + +xx_sym = sym(inputname(1)); + +% partial alpha/partial k_i +partial_xx(1) = jacobian(ALPHA_SUBS,xx_sym); +partial_xx(1) = subs(partial_xx(1),xx_sym,xx_0,0); +disp(['[computed partial alpha/partial ',inputname(1),']']); + +% partial beta/partial k_i +partial_xx(2) = jacobian(BETA_SUBS,xx_sym); +partial_xx(2) = subs(partial_xx(2),xx_sym,xx_0,0); +disp(['[computed partial beta/partial ',inputname(1),']']); + +% partial/partial k_i (partial alpha/partial z_1) +partial_xx(3) = jacobian(JAC_ALPHA_SUBS,xx_sym); +partial_xx(3) = subs(partial_xx(3),xx_sym,xx_0,0); +disp(['[computed partial/partial ',inputname(1),... + ' (partial alpha/partial z_1)]']); + + +% ------------------------------------------------------ + +function print_diff_sens(fid, partial_xx) + +% partial_alpha +clear r s +[r,s] = subexpr(partial_xx(1),'s'); +for k = 1:length(s) + fprintf(fid,'s(%s) = %s;\n',num2str(k), ... + fixlength(char(vpa(s(k),10)),'*+^',65)); +end +fprintf(fid,'\n'); +fprintf(fid,'partial_alpha = %s;\n\n', ... + fixlength(char(vpa(r,10)),'*+^',65)); + +% partial_beta +clear r s +[r,s] = subexpr(partial_xx(2),'s'); +for k = 1:length(s) + fprintf(fid,'s(%s) = %s;\n',num2str(k), ... + fixlength(char(vpa(s(k),10)),'*+^',65)); +end +fprintf(fid,'\n'); +fprintf(fid,'partial_beta = %s;\n\n', ... + fixlength(char(vpa(r,10)),'*+^',65)); + +% partial_jac_alpha +clear r s +[r,s] = subexpr(partial_xx(3),'s'); +for k = 1:length(s) + fprintf(fid,'s(%s) = %s;\n',num2str(k), ... + fixlength(char(vpa(s(k),10)),'*+^',65)); +end +fprintf(fid,'\n'); +fprintf(fid,'partial_jac_alpha = %s;\n\n', ... + fixlength(char(vpa(r,10)),'*+^',65)); + + +% ------------------------------------------------------ + +function y = replace_constants(x) + +syms ah1 ah2 ah3 ah4 + +a = controlParameters; + +ah1_ = a(1); ah2_ = a(2); ah3_ = a(3); ah4_ = a(4); + +y = subs(x,{ah1,ah2,ah3,ah4},{ah1_,ah2_,ah3_,ah4_},0); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/head_height.m b/demos/rabbit/anim/rabbit_model/head_height.m new file mode 100644 index 0000000..28dd453 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/head_height.m @@ -0,0 +1,16 @@ +function h = head_height(x) +% HEAD_HEIGHT Head height. +% H = HEAD_HEIGHT(X) + +%Eric Westervelt +%24-Mar-2001 10:46:12 + +[g,L1,L3,L4] = modelParameters; + +[n,m] = size(x); + +if n ~= 1 & m ~= 1 + h = -L3.*cos(x(:,1))-L4.*cos(x(:,3))+L1.*cos(x(:,5)); +else + h = -L3*cos(x(1))-L4*cos(x(3))+L1*cos(x(5)); +end diff --git a/demos/rabbit/anim/rabbit_model/hip_pos.m b/demos/rabbit/anim/rabbit_model/hip_pos.m new file mode 100644 index 0000000..e1b11e0 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/hip_pos.m @@ -0,0 +1,18 @@ +function [pHh,pHv] = hip_pos(x) +% HIP_POS The hip position. +% [pHv,pHh] = HIP_POS(X) + +%Eric Westervelt +%24-Mar-2001 10:46:13 + +[g,L1,L3,L4] = modelParameters; + +[n,m] = size(x); + +if n ~= 1 & m ~= 1 + pHh = L3.*sin(x(:,1))+L4.*sin(x(:,3)); + pHv = -L3.*cos(x(:,1))-L4.*cos(x(:,3)); +else + pHh = L3*sin(x(1))+L4*sin(x(3)); + pHv = -L3*cos(x(1))-L4*cos(x(3)); +end diff --git a/demos/rabbit/anim/rabbit_model/hip_vel.m b/demos/rabbit/anim/rabbit_model/hip_vel.m new file mode 100644 index 0000000..f94f7dd --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/hip_vel.m @@ -0,0 +1,12 @@ +function [vHh,vHv] = hip_vel(x) +% HIP_VEL The hip velocity. +% [vHh,vHv] = HIP_VEL(X) + +%Eric Westervelt +%24-Mar-2001 10:46:13 + +[g,L1,L3,L4] = modelParameters; + +vHh = L3.*cos(x(:,1)).*x(:,6)+L4.*cos(x(:,3)).*x(:,8); + +vHv = L3.*sin(x(:,1)).*x(:,6)+L4.*sin(x(:,3)).*x(:,8); diff --git a/demos/rabbit/anim/rabbit_model/impact_abs_red.m b/demos/rabbit/anim/rabbit_model/impact_abs_red.m new file mode 100644 index 0000000..767002b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/impact_abs_red.m @@ -0,0 +1,74 @@ +function out = impact_abs_red(u) +% +% From Jessy's code simulator. +% +q = u(1:5); +dq = u(6:10); + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = modelParameters; + +q31 = q(1); q32 = q(2); q41 = q(3); q42 = q(4); q1 = q(5); +dq31 = dq(1); dq32 = dq(2); dq41 = dq(3); dq42 = dq(4); dq1 = dq(5); + +pH = [L3*sin(q31)+L4*sin(q41); + -L3*cos(q31)-L4*cos(q41)]; +vH = [L3*cos(q31)*dq31+L4*cos(q41)*dq41; + L3*sin(q31)*dq31+L4*sin(q41)*dq41]; + +[num_r,num_c] = size(q); +if num_r > num_c + qe=[q(1:4);pH;q(5)]; + dq_minus=[dq(1:4);vH;dq(5)]; +else + qe=[q(1:4).';pH;q(5)]; + dq_minus=[dq(1:4).';vH;dq(5)]; +end + +[De,Ce,Ge,Be,Ee] = dyn_mod_abs([qe;dq_minus]); + +E = Ee(:,3:4); +% De*(dq^+ - dq^-) = E*impulsive forces=E*Fe +% E'*dq^+ = 0; +% +A = [De,-E;E',zeros(2,2)]; +B = [De*dq_minus;zeros(2,1)]; +%cond_impact = cond(A) +ans = inv(A)*B; +dq31 = ans(1); dq41 = ans(3); dz = ans(6); +vFoot_1 = -L3*sin(q31)*dq31-L4*sin(q41)*dq41+dz; % to test that after impact + % the fixed foot raises + % itself from the ground +Fe = ans(8:9); +test1 = min(100,abs(Fe(1)/Fe(2))); +test2 = vFoot_1; +if (test1 <= 2/3 & test2 >= 0) + flag = 0; + q_plus_new = [q(2);q(1);q(4);q(3);q(5)]; + dq_plus_old_coord = [ans(1:4);ans(7)]; + dq_plus_new = [dq_plus_old_coord(2);dq_plus_old_coord(1); ... + dq_plus_old_coord(4); dq_plus_old_coord(3);dq_plus_old_coord(5)]; + out = [q_plus_new;dq_plus_new;flag;test1;test2]; +elseif (test2 < 0) + flag = 1; + + disp('[IMPACT_ABS_RED.M : need abs_red_2]'); %pause HERE + + if 0 % just for debugging's sake + q_plus_new = [q(2);q(1);q(4);q(3);q(5)]; + dq_plus_old_coord = [ans(1:4);ans(7)]; + dq_plus_new = [dq_plus_old_coord(2);dq_plus_old_coord(1); ... + dq_plus_old_coord(4); dq_plus_old_coord(3); ... + dq_plus_old_coord(5)]; + out = [q_plus_new;dq_plus_new;flag;test1;test2] + end + + out = zeros(13,1); %impact_abs_red_2([q;dq]); HERE +else + flag = 1; + + q_plus_new = [q(2);q(1);q(4);q(3);q(5)]; + dq_plus_old_coord = [ans(1:4);ans(7)]; + dq_plus_new = [dq_plus_old_coord(2);dq_plus_old_coord(1); ... + dq_plus_old_coord(4); dq_plus_old_coord(3);dq_plus_old_coord(5)]; + out = [q_plus_new;dq_plus_new;flag;test1;test2]; +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/initialize.m b/demos/rabbit/anim/rabbit_model/initialize.m new file mode 100644 index 0000000..33d327d --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/initialize.m @@ -0,0 +1,16 @@ +%INITIALIZE.M +% +% Initialize the biped robot + +%Eric Westervelt +%1/6/01 + +[a,b,m,dtheta_fixed] = controlParameters; + +x0 = sigma(dtheta_fixed); + +out = impact_abs_red(x0); + +x0 = out(1:10); + +if x0(1) == 0, error('whoops!! -- check INITIALIZE.M'), end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/limb_position.m b/demos/rabbit/anim/rabbit_model/limb_position.m new file mode 100644 index 0000000..bfa293b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/limb_position.m @@ -0,0 +1,61 @@ +function out = limb_position(x,pH_horiz) +% LIMB_POSITION Head height. +% out = LIMB_POSITION(X,PH_HORIZ) +% The members are pH, pT, pHead, pFem1, pFem2, pG1, pG2, pTib1, +% pTib2, pFoot1, and pFoot2. + +%Eric Westervelt +%24-Mar-2001 10:46:12 + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4]= modelParameters; + +[n,m] = size(x); +if n == 10, k = m; else, k = n; end + +if n ~= 1 & m ~= 1 + out.pH1 = pH_horiz*ones(k,1); + out.pH2 = -L3.*cos(x(:,1))-L4.*cos(x(:,3)); + out.pT1 = out.pH1 + 1/M1.*(-sin(x(:,5)).*MZ1+cos(x(:,5)).*MY1); + out.pT2 = out.pH2 + 1/M1.*(cos(x(:,5)).*MZ1+sin(x(:,5)).*MY1); + out.pHead1 = out.pH1 + -L1.*sin(x(:,5)); + out.pHead2 = out.pH2 + L1.*cos(x(:,5)); + out.pFem11 = out.pH1 + -MZ3/M3.*sin(x(:,1)); + out.pFem12 = out.pH2 + MZ3/M3.*cos(x(:,1)); + out.pFem21 = out.pH1 + -MZ3/M3.*sin(x(:,2)); + out.pFem22 = out.pH2 + MZ3/M3.*cos(x(:,2)); + out.pG11 = out.pH1 + -L3.*sin(x(:,1)); + out.pG12 = out.pH2 + L3.*cos(x(:,1)); + out.pG21 = out.pH1 + -L3.*sin(x(:,2)); + out.pG22 = out.pH2 + L3.*cos(x(:,2)); + out.pTib11 = out.pH1 + -MZ4/M4.*sin(x(:,3))-L3.*sin(x(:,1)); + out.pTib12 = out.pH2 + MZ4/M4.*cos(x(:,3))+L3.*cos(x(:,1)); + out.pTib21 = out.pH1 + -L3.*sin(x(:,2))-MZ4/M4.*sin(x(:,4)); + out.pTib22 = out.pH2 + L3.*cos(x(:,2))+MZ4/M4.*cos(x(:,4)); + out.pFoot11 = out.pH1 + -L3.*sin(x(:,1))-L4.*sin(x(:,3)); + out.pFoot12 = out.pH2 + L3.*cos(x(:,1))+L4.*cos(x(:,3)); + out.pFoot21 = out.pH1 + -L3.*sin(x(:,2))-L4.*sin(x(:,4)); + out.pFoot22 = out.pH2 + L3.*cos(x(:,2))+L4.*cos(x(:,4)); +else + out.pH1 = pH_horiz; + out.pH2 = -L3*cos(x(1))-L4*cos(x(3)); + out.pT1 = out.pH1 + 1/M1*(-sin(x(5))*MZ1+cos(x(5))*MY1); + out.pT2 = out.pH2 + 1/M1*(cos(x(5))*MZ1+sin(x(5))*MY1); + out.pHead1 = out.pH1 + -L1*sin(x(5)); + out.pHead2 = out.pH2 + L1*cos(x(5)); + out.pFem11 = out.pH1 + -MZ3/M3*sin(x(1)); + out.pFem12 = out.pH2 + MZ3/M3*cos(x(1)); + out.pFem21 = out.pH1 + -MZ3/M3*sin(x(2)); + out.pFem22 = out.pH2 + MZ3/M3*cos(x(2)); + out.pG11 = out.pH1 + -L3*sin(x(1)); + out.pG12 = out.pH2 + L3*cos(x(1)); + out.pG21 = out.pH1 + -L3*sin(x(2)); + out.pG22 = out.pH2 + L3*cos(x(2)); + out.pTib11 = out.pH1 + -MZ4/M4*sin(x(3))-L3*sin(x(1)); + out.pTib12 = out.pH2 + MZ4/M4*cos(x(3))+L3*cos(x(1)); + out.pTib21 = out.pH1 + -L3*sin(x(2))-MZ4/M4*sin(x(4)); + out.pTib22 = out.pH2 + L3*cos(x(2))+MZ4/M4*cos(x(4)); + out.pFoot11 = out.pH1 + -L3*sin(x(1))-L4*sin(x(3)); + out.pFoot12 = out.pH2 + L3*cos(x(1))+L4*cos(x(3)); + out.pFoot21 = out.pH1 + -L3*sin(x(2))-L4*sin(x(4)); + out.pFoot22 = out.pH2 + L3*cos(x(2))+L4*cos(x(4)); +end diff --git a/demos/rabbit/anim/rabbit_model/load_all_tests.m b/demos/rabbit/anim/rabbit_model/load_all_tests.m new file mode 100644 index 0000000..9524b8d --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/load_all_tests.m @@ -0,0 +1,11 @@ +function load_all_tests() +savename=['simulation\all_tests_combined_ctrl10_test_file_June_9.mat']; +load(savename,'all_tests'); +stone_successful_steps=all_tests.stone_successful_steps; +s=0; +for i=1:100 + if stone_successful_steps(i)==10 + s=s+1; + end +end +display('Percentage of success:'); display(s); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/main_zd.m b/demos/rabbit/anim/rabbit_model/main_zd.m new file mode 100644 index 0000000..13492ad --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/main_zd.m @@ -0,0 +1,36 @@ +%MAIN_ZD.M +% +% Run simulation of walker with offset torso. + +%Eric Westervelt +%3/12/01 + +global RUN_NAME + +RUN_NAME = 't'; + +disp(['RUN_NAME = ',RUN_NAME]); + +res = []; + +if ~isempty(dir([RUN_NAME,'_zd_data.mat'])) + while isempty(res) + res = input('Do you want to over run this trial? (0/1)'); + end + if res == 0, return, end +end + +T = 3; + +initialize; + +z0 = zd_project(x0); % project initial condition onto zero dynamics + +sim('zd_model',T); + +x = zd_lift_special(z); + +save(['mat_files/',RUN_NAME,'_data'],'t','z','sim_error', ... + 'foot_touch','T'); + +anim(t,x,1/30); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/miperhr2mpersec.m b/demos/rabbit/anim/rabbit_model/miperhr2mpersec.m new file mode 100644 index 0000000..fbb4e57 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/miperhr2mpersec.m @@ -0,0 +1,14 @@ +function vsi=miperhr2mpersec(ve) +%MIPERHR2MPERSEC Convert units from mi/hr to m/sec. +% MIPERHR2MPERSEC(VE) is the velocity VE in the units of mi/hr. +% +% vsi = 100/2.54/12/5280*3600*ve +% +% Example +% vsi = miperhr2mpersec(1) +% returns 0.44704 + +%Eric Westervelt +%7/17/00 + +vsi = 5280*12*2.54/100/3600*ve; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/modelParameters.m b/demos/rabbit/anim/rabbit_model/modelParameters.m new file mode 100644 index 0000000..1079683 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/modelParameters.m @@ -0,0 +1,32 @@ +function [g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = modelParameters +%MODELPARAMETERS Model parameters for kneed biped. +% +% [G,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = MODELPARAMETERS + +%Eric Westervelt +%10/2/00 +global animation_scale +scale = animation_scale; +% gravity +g=9.81; + +% lengths +L1=0.625; +L3=0.4; +L4=0.4; + +% mass +M1=12*scale; +M3=6.8*scale; +M4=3.2*scale; + +% com +MY1=.2; +MZ1=4.; +MZ3=1.11; +MZ4=.41; + +% link inertia +XX1=2.22*scale; +XX3=1.08*scale; +XX4=.93*scale; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/mpersec2miperhr.m b/demos/rabbit/anim/rabbit_model/mpersec2miperhr.m new file mode 100644 index 0000000..6efd21a --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/mpersec2miperhr.m @@ -0,0 +1,14 @@ +function ve=mpersec2miperhr(vsi) +%MPERSEC2MIPERHR Convert units from m/sec to mi/hr. +% MPERSEC2MIPERHR(VE) is the velocity VE in the units of m/sec. +% +% ve = 1/100*2.54*12*5280/3600*vsi +% +% Example +% vsi = mpersec2miperhr(1) +% returns 0.44704 + +%Eric Westervelt +%7/17/00 + +ve = 100/2.54/12/5280*3600*vsi; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/myaa.m b/demos/rabbit/anim/rabbit_model/myaa.m new file mode 100644 index 0000000..807e6e5 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/myaa.m @@ -0,0 +1,337 @@ +function [varargout] = myaa(varargin) +%MYAA Render figure with anti-aliasing. +% MYAA +% Anti-aliased rendering of the current figure. This makes graphics look +% a lot better than in a standard matlab figure, which is useful for +% publishing results on the web or to better see the fine details in a +% complex and cluttered plot. Some simple keyboard commands allow +% the user to set the rendering quality interactively, zoom in/out and +% re-render when needed. +% +% Usage: +% myaa: Renders an anti-aliased version of the current figure. +% +% myaa(K): Sets the supersampling factor to K. Using a +% higher K yields better rendering but takes longer time. If K is +% omitted, it defaults to 4. It may be useful to run e.g. myaa(2) to +% make a low-quality rendering as a first try, because it is a lot +% faster than the default. +% +% myaa([K D]): Sets supersampling factor to K but downsamples the +% image by a factor of D. If D is larger than K, the rendered image +% will be smaller than the original. If D is smaller than K, the +% rendering will be bigger. +% +% myaa('publish'): An experimental parameter, useful for publishing +% matlab programs (see example 3). Beware, it kills the current figure. +% +% Interactivity: +% The anti-aliased figure can be updated with the following keyboard +% commands: +% +% Re-render image (to reflect changes in the figure) +% + Zoom in (decrease downsampling factor) +% - Zoom out (increase downsampling factor) +% 1 ... 9 Change supersampling and downsampling factor to ... +% q Quit, i.e. close the anti-aliased figure +% +% Myaa can also be called with up to 3 parameters. +% FIG = MYAA(K,AAMETHOD,FIGMODE) +% Parameters and output: +% K Subsampling factor. If a vector is specified, [K D], then +% the second element will describe the downsampling factor. +% Default is K = 4 and D = 4. +% AAMETHOD Downsampling method. Normally this is chosen automatically. +% 'standard': convolution based filtering and downsampling +% 'imresize': downsampling using the imresize command from +% the image toolbox. +% 'noshrink': used internally +% FIGMODE Display mode +% 'figure': the normal mode, a new figure is created +% 'update': internally used for interactive sessions +% 'publish': used internally +% FIG A handle to the new anti-aliased figure +% +% Example 1: +% spharm2; +% myaa; +% % Press '1', '2' or '4' and try '+' and '-' +% % Press 'r' or to update the anti-aliased rendering, e.g. +% % after rotating the 3-D object in the original figure. +% +% Example 2: +% line(randn(2500,2)',randn(2500,2)','color','black','linewidth',0.01) +% myaa(8); +% +% Example 3: +% xpklein; +% myaa(2,'standard'); +% +% Example 3: +% Put the following in test.m +% %% My test publish +% % Testing to publish some anti-aliased images +% % +% spharm2; % Produce some nice graphics +% myaa('publish'); % Render an anti-aliased version +% +% Then run: +% publish test.m; +% showdemo test; +% +% +% BUGS: +% Dotted and dashed lines in plots are not rendered correctly. This is +% probably due to a bug in Matlab and it will hopefully be fixed in a +% future version. +% The OpenGL renderer does not always manage to render an image large +% enough. Try the zbuffer renderer if you have problems or decrease the +% K factor. You can set the current renderer to zbuffer by running e.g. +% set(gcf,'renderer','zbuffer'). +% +% See also PUBLISH, PRINT +% +% Version 1.1, 2008-08-21 +% Version 1.0, 2008-08-05 +% +% Author: Anders Brun +% anders@cb.uu.se +% + +%% Force drawing of graphics +drawnow; + +%% Find out about the current DPI... +screen_DPI = get(0,'ScreenPixelsPerInch'); + +%% Determine the best choice of convolver. +% If IPPL is available, imfilter is much faster. Otherwise it does not +% matter too much. +try + if ippl() + myconv = @imfilter; + else + myconv = @conv2; + end +catch + myconv = @conv2; +end + +%% Set default options and interpret arguments +if isempty(varargin) + self.K = [4 4]; + try + imfilter(zeros(2,2),zeros(2,2)); + self.aamethod = 'imresize'; + catch + self.aamethod = 'standard'; + end + self.figmode = 'figure'; +elseif strcmp(varargin{1},'publish') + self.K = [4 4]; + self.aamethod = 'noshrink'; + self.figmode = 'publish'; +elseif strcmp(varargin{1},'update') + self = get(gcf,'UserData'); + figure(self.source_fig); + drawnow; + self.figmode = 'update'; +elseif strcmp(varargin{1},'lazyupdate') + self = get(gcf,'UserData'); + self.figmode = 'lazyupdate'; +elseif length(varargin) == 1 + self.K = varargin{1}; + if length(self.K) == 1 + self.K = [self.K self.K]; + end + if self.K(1) > 16 + error('To avoid excessive use of memory, K has been limited to max 16. Change the code to fix this on your own risk.'); + end + try + imfilter(zeros(2,2),zeros(2,2)); + self.aamethod = 'imresize'; + catch + self.aamethod = 'standard'; + end + self.figmode = 'figure'; +elseif length(varargin) == 2 + self.K = varargin{1}; + self.aamethod = varargin{2}; + self.figmode = 'figure'; +elseif length(varargin) == 3 + self.K = varargin{1}; + self.aamethod = varargin{2}; + self.figmode = varargin{3}; + if strcmp(self.figmode,'publish') && ~strcmp(varargin{2},'noshrink') + printf('\nThe AAMETHOD was not set to ''noshrink'': Fixed.\n\n'); + self.aamethod = 'noshrink'; + end +else + error('Wrong syntax, run: help myaa'); +end + +if length(self.K) == 1 + self.K = [self.K self.K]; +end + +%% Capture current figure in high resolution +if ~strcmp(self.figmode,'lazyupdate'); + tempfile = 'myaa_temp_screendump.png'; + self.source_fig = gcf; + current_paperpositionmode = get(self.source_fig,'PaperPositionMode'); + current_inverthardcopy = get(self.source_fig,'InvertHardcopy'); + set(self.source_fig,'PaperPositionMode','auto'); + set(self.source_fig,'InvertHardcopy','off'); + print(self.source_fig,['-r',num2str(screen_DPI*self.K(1))], '-dpng', tempfile); + set(self.source_fig,'InvertHardcopy',current_inverthardcopy); + set(self.source_fig,'PaperPositionMode',current_paperpositionmode); + self.raw_hires = imread(tempfile); + delete(tempfile); +end +%% Start filtering to remove aliasing +w = warning; +warning off; +if strcmp(self.aamethod,'standard') || strcmp(self.aamethod,'noshrink') + % Subsample hires figure image with standard anti-aliasing using a + % butterworth filter + kk = lpfilter(self.K(2)*3,self.K(2)*0.9,2); + mm = myconv(ones(size(self.raw_hires(:,:,1))),kk,'same'); + a1 = max(min(myconv(single(self.raw_hires(:,:,1))/(256),kk,'same'),1),0)./mm; + a2 = max(min(myconv(single(self.raw_hires(:,:,2))/(256),kk,'same'),1),0)./mm; + a3 = max(min(myconv(single(self.raw_hires(:,:,3))/(256),kk,'same'),1),0)./mm; + if strcmp(self.aamethod,'standard') + if abs(1-self.K(2)) > 0.001 + raw_lowres = double(cat(3,a1(2:self.K(2):end,2:self.K(2):end),a2(2:self.K(2):end,2:self.K(2):end),a3(2:self.K(2):end,2:self.K(2):end))); + else + raw_lowres = self.raw_hires; + end + else + raw_lowres = double(cat(3,a1,a2,a3)); + end +elseif strcmp(self.aamethod,'imresize') + % This is probably the fastest method available at this moment... + raw_lowres = single(imresize(self.raw_hires,1/self.K(2),'bilinear'))/256; +end +warning(w); + +%% Place the anti-aliased image in some image on the screen ... +if strcmp(self.figmode,'figure'); + % Create a new figure at the same place as the previous + % The content of this new image is just a bitmap... + oldpos = get(gcf,'Position'); + self.myaa_figure = figure; + fig = self.myaa_figure; + set(fig,'Menubar','none'); + set(fig,'Resize','off'); + sz = size(raw_lowres); + set(fig,'Units','pixels'); + pos = [oldpos(1:2) sz(2:-1:1)]; + set(fig,'Position',pos); + ax = axes; + image(raw_lowres); + set(ax,'Units','pixels'); + set(ax,'Position',[1 1 sz(2) sz(1)]); + axis off; +elseif strcmp(self.figmode,'publish'); + % Create a new figure at the same place as the previous + % The content of this new image is just a bitmap... + self.myaa_figure = figure; + fig = self.myaa_figure; + current_units = get(self.source_fig,'Units'); + set(self.source_fig,'Units','pixels'); + pos = get(self.source_fig,'Position'); + set(self.source_fig,'Units',current_units); + set(fig,'Position',[pos(1) pos(2) pos(3) pos(4)]); + ax = axes; + image(raw_lowres); + set(ax,'Units','normalized'); + set(ax,'Position',[0 0 1 1]); + axis off; + close(self.source_fig); +elseif strcmp(self.figmode,'update'); + fig = self.myaa_figure; + figure(fig); + clf; + set(fig,'Menubar','none'); + set(fig,'Resize','off'); + sz = size(raw_lowres); + set(fig,'Units','pixels'); + pos = get(fig,'Position'); + pos(3:4) = sz(2:-1:1); + set(fig,'Position',pos); + ax = axes; + image(raw_lowres); + set(ax,'Units','pixels'); + set(ax,'Position',[1 1 sz(2) sz(1)]); + axis off; +elseif strcmp(self.figmode,'lazyupdate'); + clf; + fig = self.myaa_figure; + sz = size(raw_lowres); + pos = get(fig,'Position'); + pos(3:4) = sz(2:-1:1); + set(fig,'Position',pos); + ax = axes; + image(raw_lowres); + set(ax,'Units','pixels'); + set(ax,'Position',[1 1 sz(2) sz(1)]); + axis off; +end + +%% Store current state + +set(gcf,'userdata',self); +set(gcf,'KeyPressFcn',@keypress); +set(gcf,'Interruptible','off'); + +%% Avoid unnecessary console output +if nargout == 1 + varargout(1) = {fig}; +end + +%% A simple lowpass filter kernel (Butterworth). +% sz is the size of the filter +% subsmp is the downsampling factor to be used later +% n is the degree of the butterworth filter +function kk = lpfilter(sz, subsmp, n) +sz = 2*floor(sz/2)+1; % make sure the size of the filter is odd +cut_frequency = 0.5 / subsmp; +range = (-(sz-1)/2:(sz-1)/2)/(sz-1); +[ii,jj] = ndgrid(range,range); +rr = sqrt(ii.^2+jj.^2); +kk = ifftshift(1./(1+(rr./cut_frequency).^(2*n))); +kk = fftshift(real(ifft2(kk))); +kk = kk./sum(kk(:)); + +function keypress(src,evnt) +if isempty(evnt.Character) + return +end +recognized = 0; +self = get(gcf,'userdata'); + +if evnt.Character == '+' + self.K(2) = max(self.K(2).*0.5^(1/2),1); + recognized = 1; + set(gcf,'userdata',self); + myaa('lazyupdate'); +elseif evnt.Character == '-' + self.K(2) = min(self.K(2).*2^(1/2),16); + recognized = 1; + set(gcf,'userdata',self); + myaa('lazyupdate'); +elseif evnt.Character == ' ' || evnt.Character == 'r' || evnt.Character == 'R' + set(gcf,'userdata',self); + myaa('update'); +elseif evnt.Character == 'q' + close(gcf); +elseif find('123456789' == evnt.Character) + self.K = [str2double(evnt.Character) str2double(evnt.Character)]; + set(gcf,'userdata',self); + myaa('update'); +end + + + + + diff --git a/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens.m b/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens.m new file mode 100644 index 0000000..0ae7d4b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens.m @@ -0,0 +1,17 @@ +% PLOT_CTRL_PARAM_SENS.M +% +%Eric Westervelt +%12/10/00 + +fig_hl = figure(1); +clf +set(fig_hl,'Position',[50 290 400 400]); +set(fig_hl,'PaperPosition',[1.25 1.5 6 8]) + +vectfield('zd_diff',2.85:0.05:3.38,-2:.1:0,1); + +hold on +xlabel('z1 \rightarrow 1/2(q_{31}+q_{41})'); +ylabel('z2 \rightarrow really messy expression'); +title('differential vector field'); +grid; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens_cc.m b/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens_cc.m new file mode 100644 index 0000000..073c65e --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens_cc.m @@ -0,0 +1,17 @@ +% PLOT_CTRL_PARAM_SENS.M +% +%Eric Westervelt +%12/10/00 + +fig_hl = figure(1); +clf +set(fig_hl,'Position',[50 290 400 400]); +set(fig_hl,'PaperPosition',[1.25 1.5 6 8]) + +vectfield('zd_diff_cc',2.8:0.05:3.5,-2:.1:0,.99999,1); + +hold on +xlabel('z1 \rightarrow 1/2(q_{31}+q_{41})'); +ylabel('z2 \rightarrow 1/2(dq_{31}+dq_{41})'); +title('differential vector field'); +grid; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/poincare_main.m b/demos/rabbit/anim/rabbit_model/poincare_main.m new file mode 100644 index 0000000..6a1b38e --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/poincare_main.m @@ -0,0 +1,31 @@ +function [lambda_omega] = poincare_main(min_omega,max_omega,pts) +%POINCARE_MAIN Generate the Poincare map. +% [LAMBDA_OMEGA] = POINCARE_MAIN(MIN_OMEGA, MAX_OMEGA, PTS) +% is the Poincare map from MIN_OMEGA to MAX_OMEGA with PTS +% number of points. + +%Eric Westervelt +%10/2/00 + +omega = linspace(min_omega,max_omega,pts); + +lambda_omega = walk(0,0,'poincare',omega); + +if 1 + figure(1) + clf + subplot(2,1,1) + plot(omega,omega,omega,lambda_omega,'--') + xlabel('\omega^-'); + ylabel('\lambda(\omega^-)'); + %axis([min(omega) max(omega) 1.2 2]) + + subplot(2,1,2) + plot(omega,omega*0,omega,lambda_omega-omega,'x') + xlabel('\omega^-'); + ylabel('\lambda(\omega^-)-\omega^-'); + %axis([min(omega) max(omega) -0.3 0.3]) +end + + + diff --git a/demos/rabbit/anim/rabbit_model/poincare_zd_compare.m b/demos/rabbit/anim/rabbit_model/poincare_zd_compare.m new file mode 100644 index 0000000..ba1fdee --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/poincare_zd_compare.m @@ -0,0 +1,85 @@ +function poincare_zd_compare(omega_start,omega_end,pts) +%POINCARE_ZD_COMPARE Make some useful plots showing the +% equivalence of the zero dynamics and the full system's dynamics. +% +% POINCARE_ZD_COMPARE(OMEGA_START, OMEGA_END, PTS) is the +% comparison Poincare map from OMEGA_START to OMEGA_END with +% PTS number of points. + +%Eric Westervelt +%10/29/00 + +if 1 +% Make comparison of Poincare map of full system and zero dynamics +% system +% +l_full = poincare_main(omega_start,omega_end,pts); +l_zd = poincare_zd_main(omega_start,omega_end,pts); + +l_full - l_zd + +fig_hl = figure(2); +clf +screen_position = [330 100 600 700]; +paper_position = [1 1.25 6.5 8.5]; + +%screen_position = [30 100 600 300]; % for presentation +%paper_position = [1.25 1.5 10 6]; % for presentation + +set(fig_hl,'Position', screen_position); +set(fig_hl,'PaperPosition',paper_position) +set(fig_hl,'PaperOrientation','portrait') + +omega = linspace(omega_start,omega_end,pts); + +plot(omega,omega,'b-',omega,l_full,'r--',omega,l_zd,'g-.') +xlabel('\omega^-'); +ylabel('\lambda(\omega^-)'); +legend('identity line','full model','zero dynamics',2); +title('Comparison of full model''s and zero dynamics'' Poincare maps'); +axis([omega_start omega_end omega_start omega_end]); + +drawnow + +print plots/poincare_zd_comparison.ps +return +end + + +%THIS CODE HASN'T BEEN USED IN A WHILE...MAKE TAKE SOME DOING TO +%RESURRECT IT (2/16/01) +% +%Show the zero dynamics are identical to the full systems dynamics +%once the controller has converged. +% +global N + +fig_hl = figure(1); +clf +screen_position = [330 100 600 700]; +paper_position = [1 1.25 6.5 8.5]; +set(fig_hl,'Position', screen_position); +set(fig_hl,'PaperPosition',paper_position) + +disp('using data from file '); +s = load('mat_files/high_gain_data'); + +for k = 1:6 + N = k*10; + + [t,z] = sim_zd; + + subplot(3,2,k); + plot(s.t,1/2*(s.x(:,1)+s.x(:,3)),'-', ... + t+s.t(N),z(:,1),'--', ... + s.t(N),1/2*(s.x(N,1)+s.x(N,3)),'o'); + str = sprintf('%.4f',s.t(N)); + title(['t0 = ',str]) + if k == 1 + legend('actual dyn (high gain)','zero dyn','zero dyn ic') + end +end + +N = []; + +%print plots/almost_lin_zd_high_gain.ps diff --git a/demos/rabbit/anim/rabbit_model/poincare_zd_compare_cc.m b/demos/rabbit/anim/rabbit_model/poincare_zd_compare_cc.m new file mode 100644 index 0000000..9a64683 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/poincare_zd_compare_cc.m @@ -0,0 +1,78 @@ +%POINCARE_ZD_COMPARE_CC Make some useful plots showing the +%equivalence of the zero dynamics and the full system's dynamics. + +%Eric Westervelt +%1/24/01 + +% Make comparison of Poincare map of full system and zero dynamics +% system +% +min_omega = -1.8; +max_omega = -1.4; + pts = 5; + +l_full = poincare_main(min_omega,max_omega,pts); +l_zd = poincare_zd_main_cc(min_omega,max_omega,pts); + +l_full - l_zd + +fig_hl = figure(2); +clf +screen_position = [330 100 600 700]; +paper_position = [1 1.25 6.5 8.5]; +set(fig_hl,'Position', screen_position); +set(fig_hl,'PaperPosition',paper_position) +set(fig_hl,'PaperOrientation','portrait') + +omega = linspace(min_omega,max_omega,pts); + +plot(omega,omega,'b-',omega,l_full,'r--',omega,l_zd,'g-.') +xlabel('\omega^-'); +ylabel('\lambda(\omega^-)'); +legend('identity line','full model','zero dynamics',2); +title('Comparison of full model''s and zero dynamics'' Poincare maps'); +axis([min_omega max_omega min_omega max_omega]); + +drawnow + +print plots/poincare_zd_comparison_cc.ps + + +return + +%THIS CODE HASN'T BEEN USED IN A WHILE...MAKE TAKE SOME DOING TO +%RESURRECT IT (2/16/01) +% +%Show the zero dynamics are identical to the full systems dynamics +%once the controller has converged. +% +global N + +fig_hl = figure(1); +clf +screen_position = [330 100 600 700]; +paper_position = [1 1.25 6.5 8.5]; +set(fig_hl,'Position', screen_position); +set(fig_hl,'PaperPosition',paper_position) + +disp('using data from file '); +s = load('mat_files/high_gain_data'); + +for k = 1:6 + N = k*15; + + [t,z] = sim_zd; + + subplot(3,2,k); + plot(s.t,1/2*(s.x(:,3)+s.x(:,1)),'-', ... + t+s.t(N),z(:,1),'--', ... + s.t(N),1/2*(s.x(N,3)+s.x(N,1)),'o'); + str = sprintf('%.4f',s.t(N)); + title(['t0 = ',str]) + if k == 1 + legend('actual dyn (high gain)','zero dyn','zero dyn ic') + end +end + +%print plots/almost_lin_zd_high_gain.ps + diff --git a/demos/rabbit/anim/rabbit_model/poincare_zd_main.m b/demos/rabbit/anim/rabbit_model/poincare_zd_main.m new file mode 100644 index 0000000..47b3817 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/poincare_zd_main.m @@ -0,0 +1,31 @@ +function [lam_omega] = poincare_zd_main(omega_start,omega_end,pts) +%POINCARE_ZD_MAIN Generate the Poincare map from zero dynamics. +% [LAM_OMEGA] = POINCARE_ZD_MAIN(OMEGA_START, OMEGA_END, PTS) +% is the Poincare map from OMEGA_START to OMEGA_END with PTS +% number of points. + +%Eric Westervelt +%11/3/00 + +omega = linspace(omega_start,omega_end,pts); + +lam_omega = sim_zd(0,0,'poincare',omega); + +if 1 + figure(2) + %clf + subplot(2,1,1) + hold on + plot(omega,omega,omega,lam_omega,'--') + xlabel('\omega^-'); + ylabel('\lambda(\omega^-)'); + + subplot(2,1,2) + hold on + plot(omega,omega*0,omega,lam_omega-omega,'x') + xlabel('\omega^-'); + ylabel('\lambda(\omega^-)-\omega^-'); +end + + + diff --git a/demos/rabbit/anim/rabbit_model/poincare_zd_main_cc.m b/demos/rabbit/anim/rabbit_model/poincare_zd_main_cc.m new file mode 100644 index 0000000..cadfba4 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/poincare_zd_main_cc.m @@ -0,0 +1,31 @@ +function [lam_omega] = poincare_zd_main_cc(omega_start,omega_end,pts) +%POINCARE_ZD_MAIN Generate the Poincare map from zero dynamics. +% [LAM_OMEGA] = POINCARE_ZD_MAIN(OMEGA_START, OMEGA_END, PTS) +% is the Poincare map from OMEGA_START to OMEGA_END with PTS +% number of points. + +%Eric Westervelt +%11/3/00 + +omega = linspace(omega_start,omega_end,pts); + +lam_omega = sim_zd_cc(0,0,'poincare',omega); + +if 1 + figure(2) + %clf + subplot(2,1,1) + hold on + plot(omega,omega,omega,lam_omega,'--') + xlabel('\omega^-'); + ylabel('\lambda(\omega^-)'); + + subplot(2,1,2) + hold on + plot(omega,omega*0,omega,lam_omega-omega,'x') + xlabel('\omega^-'); + ylabel('\lambda(\omega^-)-\omega^-'); +end + + + diff --git a/demos/rabbit/anim/rabbit_model/quiver_erw.m b/demos/rabbit/anim/rabbit_model/quiver_erw.m new file mode 100644 index 0000000..4ea257b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/quiver_erw.m @@ -0,0 +1,106 @@ +function hh = quiver_erw(varargin) +%QUIVER_ERW Quiver plot with different head size (only changed +%alpha and beta). + +%Eric Westervelt +%12/11/00 + +% Arrow head parameters +alpha = .3; % Size of arrow head relative to the length of the vector +beta = .1; % Width of the base of the arrow head relative to the length +%autoscale = 1; % Autoscale if ~= 0 then scale by this. +plotarrows = 1; % Plot arrows +sym = ''; + +filled = 0; +ls = '-'; +ms = ''; +col = ''; + +nin = nargin; +% Parse the string inputs +while isstr(varargin{nin}), + vv = varargin{nin}; + if ~isempty(vv) & strcmp(lower(vv(1)),'f') + filled = 1; + nin = nin-1; + else + [l,c,m,msg] = colstyle(vv); + if ~isempty(msg), + error(sprintf('Unknown option "%s".',vv)); + end + if ~isempty(l), ls = l; end + if ~isempty(c), col = c; end + if ~isempty(m), ms = m; plotarrows = 0; end + if isequal(m,'.'), ms = ''; end % Don't plot '.' + nin = nin-1; + end +end + +error(nargchk(2,5,nin)); + +% Check numeric input arguments +if nin<4, % quiver(u,v) or quiver(u,v,s) + [msg,x,y,u,v] = xyzchk(varargin{1:2}); +else + [msg,x,y,u,v] = xyzchk(varargin{1:4}); +end +if ~isempty(msg), error(msg); end + +if nin==3 | nin==5, % quiver(u,v,s) or quiver(x,y,u,v,s) + autoscale = varargin{nin}; +end + +% Scalar expand u,v +if prod(size(u))==1, u = u(ones(size(x))); end +if prod(size(v))==1, v = v(ones(size(u))); end + +%if autoscale, +% Base autoscale value on average spacing in the x and y +% directions. Estimate number of points in each direction as +% either the size of the input arrays or the effective square +% spacing if x and y are vectors. +if min(size(x))==1, n=sqrt(prod(size(x))); m=n; else [m,n]=size(x); end +delx = diff([min(x(:)) max(x(:))])/n; +dely = diff([min(y(:)) max(y(:))])/m; +len = sqrt((u.^2 + v.^2)/(delx.^2 + dely.^2)); +autoscale = autoscale*0.02;% / max(len(:)); +u = u*autoscale; v = v*autoscale; +%end + +ax = newplot; +next = lower(get(ax,'NextPlot')); +hold_state = ishold; + +% Make velocity vectors +x = x(:).'; y = y(:).'; +u = u(:).'; v = v(:).'; +uu = [x;x+u;repmat(NaN,size(u))]; +vv = [y;y+v;repmat(NaN,size(u))]; + +h1 = plot(uu(:),vv(:),[col ls]); + +if plotarrows, + % Make arrow heads and plot them + hu = [x+u-alpha*(u+beta*(v+eps));x+u; ... + x+u-alpha*(u-beta*(v+eps));repmat(NaN,size(u))]; + hv = [y+v-alpha*(v-beta*(u+eps));y+v; ... + y+v-alpha*(v+beta*(u+eps));repmat(NaN,size(v))]; + hold on + h2 = plot(hu(:),hv(:),[col ls]); +else + h2 = []; +end + +if ~isempty(ms), % Plot marker on base + hu = x; hv = y; + hold on + h3 = plot(hu(:),hv(:),[col ms]); + if filled, set(h3,'markerfacecolor',get(h1,'color')); end +else + h3 = []; +end + +if ~hold_state, hold off, view(2); set(ax,'NextPlot',next); end + +if nargout>0, hh = [h1;h2;h3]; end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/rm_gaps.m b/demos/rabbit/anim/rabbit_model/rm_gaps.m new file mode 100644 index 0000000..d31d224 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/rm_gaps.m @@ -0,0 +1,41 @@ +function [t_new,varargout] = rm_gaps(t,varargin) +%RM_GAPS Remove gaps. +% [T_NEW,VARARGOUT] = RM_GAPS(T,VARARGIN) removes the gaps +% created by a variable step integration routine from t +% and from all other column vectors given in varargin. + +%Eric Westervelt +%6/29/00 + +nargout = nargin; + + +if length(t) < 3 + disp('Not enough data points to warrent removing of gaps.') + t_new = t; + varargout = varargin; + return +end + +k = length(t) - 1; t_new = t(k+1); +for k2 = 1:nargin-1 + varargout{k2} = varargin{k2}(k+1,:); +end +while k > 0 + if t(k) < t(k+1) + t_new = [t(k); t_new]; + for k2 = 1:nargin-1 + varargout{k2} = [varargin{k2}(k,:); varargout{k2}]; + end + else + k2 = k + 1; + while t(k) >= t(k2) + k = k - 1; + if k == 0 %KSG [02MAY12]: If there are a string of zeros in the time vector, this prevents a call to t(0) + break + end + end + k = k + 1; + end + k = k - 1; +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/sigma.m b/demos/rabbit/anim/rabbit_model/sigma.m new file mode 100644 index 0000000..d950514 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/sigma.m @@ -0,0 +1,37 @@ +function x0 = sigma(dtheta1,theta1) +%SIGMA Maps horizontal velocity of hips just before impact to +% state of the system just before impact. +% +% X0 = SIGMA(DTHETA1,THETA1) + +%Eric Westervelt +%01-May-2001 05:42:39 + +[a,b,m] = controlParameters; + +Ah_inv = zeros(5); +Ah_inv(1,3)=-0.5; +Ah_inv(1,5)=1; +Ah_inv(2,2)=1; +Ah_inv(2,3)=-0.5; +Ah_inv(2,5)=1; +Ah_inv(3,3)=0.5; +Ah_inv(3,5)=1; +Ah_inv(4,2)=1; +Ah_inv(4,3)=-0.5; +Ah_inv(4,4)=1; +Ah_inv(4,5)=1; +Ah_inv(5,1)=1; + +aN = a([7;14;21;28]); +aN_minus_1 = a([6;13;20;27]); + +if nargin == 2 + q = Ah_inv*[aN; theta1]; +else + q = Ah_inv*[aN; b+m]; +end + +dq = Ah_inv*[-6*(aN_minus_1-aN)/m; 1]*dtheta1; + +x0 = [q; dq]; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/sigma_vel.m b/demos/rabbit/anim/rabbit_model/sigma_vel.m new file mode 100644 index 0000000..c57631b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/sigma_vel.m @@ -0,0 +1,29 @@ +function dq = sigma_vel(dtheta1) +%SIGMA_VEL Maps horizontal velocity of hips just before impact to +% state of the system just before impact. +% +% DQ = SIGMA_VEL(DTHETA1) + +%Eric Westervelt +%01-May-2001 05:42:40 + +[a,b,m] = controlParameters; + +Ah_inv = zeros(5); +Ah_inv(1,3)=-0.5; +Ah_inv(1,5)=1; +Ah_inv(2,2)=1; +Ah_inv(2,3)=-0.5; +Ah_inv(2,5)=1; +Ah_inv(3,3)=0.5; +Ah_inv(3,5)=1; +Ah_inv(4,2)=1; +Ah_inv(4,3)=-0.5; +Ah_inv(4,4)=1; +Ah_inv(4,5)=1; +Ah_inv(5,1)=1; + +aN = a([7;14;21;28]); +aN_minus_1 = a([6;13;20;27]); + +dq = Ah_inv*[-6*(aN_minus_1-aN)/m; 1]*dtheta1; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/sim_zd.m b/demos/rabbit/anim/rabbit_model/sim_zd.m new file mode 100644 index 0000000..8c83b6e --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/sim_zd.m @@ -0,0 +1,266 @@ +function varargout = sim_zd(t,x,flag,opt_param) +%SIM_ZD Simulate the zero dynamics of the kneed biped walker. + +%Eric Westervelt +%10/26/00 + +if nargin == 0 + flag = 'demo'; +end + +switch flag + case '' % Return dx/dt = dynamics(t,x). + varargout{1} = f(t,x); + case 'init' % Return default [tspan,x0,options]. + [varargout{1:3}] = init; + case 'events' % Return [value,isterminal,direction]. + [varargout{1:3}] = events(t,x); + case 'poincare' % Return [lam_omega]. + varargout{1} = poincare(opt_param); + case 'demo' % Run a demo. + [varargout{1:5}] = demo; + otherwise + error(['Unknown flag ''' flag '''.']); +end + + +% -------------------------------------------------------------------------- +% This is the system dynamics function + +function dz = f(t,z) + +dz = zd(z); + +%[dx,dx_approx] = zd(x); +%dx = dx_approx; + + +% -------------------------------------------------------------------------- +% This is the init function + +function [tspan,x0,options] = init + +error('Should not use init function without first setting it up.'); + + +% -------------------------------------------------------------------------- +% This is the events function + +function [value,isterminal,direction] = events(t,z) +%Locate the time when critial angle of stance leg minus stance leg +%angle passes through zero in a decreasing direction and stop +%integration. + +global SIM_COUNTER + +SIM_COUNTER = SIM_COUNTER + 1; + +dz = zd(z); +x = zd_lift(z(1),dz(1)).'; + +if SIM_COUNTER > 200 & mod(t,.01) + value(9) = 1; +else + value(9) = 0; +end + +[pHh,pHv]=hip_pos(x); + +h = swing_foot_height(x'); + +value(1) = h; +value(2) = x(5) - pi/6; % torso not too far backward +value(3) = x(5) + pi/6; % torso not too far forward +value(4) = x(2) - 7*pi/10; % swing femur not too far back +value(5) = x(2) - 13*pi/10; % swing femur not too far forward +value(6) = pHv - .6; % hips not too low +value(7) = x(1) - x(3) - pi/2; % stance knee not too bent +value(8) = x(2) - x(4) - pi/2; % swing knee not too bent + +isterminal(1) = 1; % stop when this event occurs +isterminal(2) = 1; % stop " +isterminal(3) = 1; % stop " +isterminal(4) = 1; % stop " +isterminal(5) = 1; % stop " +isterminal(6) = 1; % stop " +isterminal(7) = 1; % stop " +isterminal(8) = 1; % stop " +isterminal(9) = 1; % stop " + +direction(1) = -1; % decreasing direction +direction(2) = 0; % decreasing " +direction(3) = 0; % decreasing " +direction(4) = 0; % decreasing " +direction(5) = 0; % increasing " +direction(6) = 0; % decreasing " +direction(7) = 0; % increasing " +direction(8) = 0; % increasing " +direction(9) = 0; % increasing " + + +% -------------------------------------------------------------------------- +% This is the Poincare map function + +function [lam_omega] = poincare(opt_param) +% Generate the Poincare map for a vector opt_param + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = ... + modelParameters; + +options = odeset('Events','on', ... + 'Refine',4, ... + 'MaxStep',.008, ... + 'RelTol',10^-5, ... + 'AbsTol',10^-6); + +n = length(opt_param); +lam_omega = []; + +for k = 1:length(opt_param), + disp([num2str(k/n*100,'%0.2f'),'% done generating Poincare map']); + + x0 = sigma(opt_param(k)); + out = impact_abs_red(x0).'; + x0 = out(1:10); + + % project i.c. onto zero dynamics + z0 = zd_project(x0); + + [t,z] = ode45('sim_zd',[0 2],z0,options); + nt = length(t); + + x = zd_lift_special(z(nt,:)); + + % point invalid if swing foot is behind stance foot at end of "step" + if step_length(x) <= 0 + disp('Stepping error ... didn''t step properly..'); + lam_omega(k) = 0; % outside region of attraction + else + lam_omega(k) = 1/2*(x(6)+x(8)); + end +end + + +% -------------------------------------------------------------------------- +% This is the demo function + +function [tout,zout,teout,zeout,ieout] = demo + +global SIM_COUNTER +global N % specified in + +SIM_COUNTER = 0; + +if ~isempty(N) + %THIS CODE HASN'T BEEN USED IN A WHILE...MAKE TAKE SOME DOING TO + %RESURRECT IT (2/16/01) + + s = load('mat_files/high_gain_data'); + z0 = zd_project(s.x(N,:))'; + + tstart = 0; + tfinal = s.t(length(s.t)) - s.t(N); + num_steps = 1; + zout = z0'; +else + % initial condtion in full coordinates + initialize; + + % project initial condition onto zero dynamics + z0 = zd_project(x0); + + num_steps = 1; + zout = z0; +end + +tstart = 0; +tfinal = 20; + +tout = tstart; + +step_t = []; + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = ... + modelParameters; + +options = odeset('Events','on', ... + 'Refine',4, ... + 'MaxStep',.008, ... + 'RelTol',10^-5, ... + 'AbsTol',10^-6); + +teout = []; zeout = []; ieout = []; + +for i = 1:num_steps + % Solve until the first terminal event. + [t,z,te,ze,ie] = ode45('sim_zd',[tstart tfinal],z0,options); + + nt = length(t); + + % Accumulate output. This could be passed out as output + % arguments. + tout = [tout; t(2:nt)]; + zout = [zout; z(2:nt,:)]; + teout = [teout; te]; + zeout = [zeout; ze]; + ieout = [ieout; ie]; + + if t(nt) <= tfinal & i ~= num_steps + % Set the new initial conditions (after impact). + dz = zd(z(nt,:)); + xf = zd_lift(z(nt,1),dz(1)); + x0 = impact_abs_red(xf); + z0 = zd_project(x0(1:10)'); + + % monitor flag from Grizzle's impact function + if x0(11), disp('stepping error -- IMPACT_ABS_RED.M'); end + + step_len = step_length(xf); + + % Diplay some useful info + im = sprintf('%.5f',x0(12)); + dt = sprintf('%.5f',t(length(t))-t(1)); + sl = sprintf('%.5f',step_len); + ra = sprintf('%.5f',step_len/(t(length(t))-t(1))); + disp(['step: ',num2str(i),', impact: ',im, ... + ', delta_t: ',dt,', length: ',sl,', rate: ',ra]); + + step_t = [step_t t(length(t))]; + + % Stop simulation if swing foot is behind stance foot at end of + % "step" + if step_len <= 0 + disp('stepping error - swing foot didn''t swing forward.'); + end + end + + tstart = t(nt); + STEP_TIME = t(nt); + if tstart >= tfinal + break + end +end + +ieout + +if 0 + xout = zd_lift_special(zout); + anim(tout,xout,1/20) + + disp('[Done with simulation, about to draw zero dynamics'' plot]'); + + % use this when you run a lot of steps to see the limit cycle + % form in the zero dynamics!! + fig_hl=figure(2); + clf + set(fig_hl,'Position',[100 100 350 700]); + set(fig_hl,'PaperPosition',[1.25 1.5 6 8]) + + vectfield('zd',2.8:0.04:3.5,-40:.5:-15,.999); + hold on + plot(zout(:,1),zout(:,2)); + xlabel('z_1 \rightarrow q41'); + ylabel('z_2 \rightarrow really messy expression'); + title('Limit cycle of zero dynamics overlayed on vector field'); + grid; +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/sim_zd_cc.m b/demos/rabbit/anim/rabbit_model/sim_zd_cc.m new file mode 100644 index 0000000..69a428b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/sim_zd_cc.m @@ -0,0 +1,213 @@ +function varargout = sim_zd_cc(t,x,flag,opt_param) +%SIM_ZD_CC Simulate the zero dynamics of the kneed biped walker. + +%Eric Westervelt +%1/22/01 + +if nargin == 0 + flag = 'demo'; +end + +switch flag +case '' % Return dx/dt = dynamics(t,x). + varargout{1} = f(t,x); +case 'init' % Return default [tspan,x0,options]. + [varargout{1:3}] = init; +case 'events' % Return [value,isterminal,direction]. + [varargout{1:3}] = events(t,x); +case 'poincare' % Return [lam_omega]. + varargout{1} = poincare(opt_param); +case 'demo' % Run a demo. + [varargout{1:2}] = demo; +otherwise + error(['Unknown flag ''' flag '''.']); +end + + +% -------------------------------------------------------------------------- +% This is the system dynamics function + +function dz = f(t,z) + +dz = zd_cc(z); + +% -------------------------------------------------------------------------- +% This is the init function + +function [tspan,x0,options] = init + +error('Should not use init function without first setting it up.'); + + +% -------------------------------------------------------------------------- +% This is the events function + +function [value,isterminal,direction] = events(t,z) +% Locate the time when critial angle of stance leg minus stance leg +% angle passes through zero in a decreasing direction and stop +% integration. + +x = zd_lift(z(1),z(2))'; + +% when swing leg foot touches ground +value(1) = swing_foot_height(x); + +isterminal(1) = 1; % stop when this event occurs +direction(1) = -1; % decreasing direction detection + + +% -------------------------------------------------------------------------- +% This is the Poincare map function + +function [lam_omega] = poincare(opt_param) +% Generate the Poincare map for a vector opt_param + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = ... + modelParameters; + +options = odeset('Events','on', ... + 'Refine',5, ... + 'RelTol',10^-5, ... + 'AbsTol',10^-6); + +n = length(opt_param); +lam_omega = []; + +for k = 1:length(opt_param), + disp([num2str(k/n*100,'%0.2f'),'% done generating Poincare map']); + + x0 = sigma(opt_param(k)); + out = impact_abs_red(x0).'; + x0 = out(1:10); + + % project i.c. onto zero dynamics + z0 = zd_project_cc(x0); + + [t,z] = ode45('sim_zd_cc',[0 2],z0,options); + nt = length(t); + + x = zd_lift(z(nt,1),z(nt,2)); + + % point invalid if swing foot is behind stance foot at end of "step" + if step_length(x) <= 0 + disp('Stepping error ... didn''t step properly..'); + lam_omega(k) = 0; % outside region of attraction + else + lam_omega(k) = 1/2*(x(6)+x(8)); + end +end + + +% -------------------------------------------------------------------------- +% This is the demo function + +function [tout,zout] = demo + +if 0 + %THIS CODE HASN'T BEEN USED IN A WHILE...MAKE TAKE SOME DOING TO + %RESURRECT IT (2/16/01) + + global N % specified in + + s = load('mat_files/high_gain_data'); + z0 = zd_project_cc(s.x(N,:))'; + + tstart = 0; + tfinal = s.t(length(s.t)) - s.t(N); + num_steps = 1; + zout = z0'; +else + % initial condtion in full coordinates + initialize; + + % project initial condition onto zero dynamics + z0 = zd_project_cc(x0); + num_steps = 2; + zout = z0; +end + +tstart = 0; +tfinal = 50; + +tout = tstart; + +step_t = []; + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4] = ... + modelParameters; + +options = odeset('Events','on', ... + 'Refine',4, ... + 'RelTol',10^-5, ... + 'AbsTol',10^-6); + +for i = 1:num_steps + % Solve until the first terminal event. + [t,z] = ode45('sim_zd_cc',[tstart tfinal],z0,options); + + nt = length(t); + + % Accumulate output. This could be passed out as output arguments. + tout = [tout; t(2:nt)]; + zout = [zout; z(2:nt,:)]; + + if t(nt) <= tfinal + % Set the new initial conditions (after impact). + xf = zd_lift(z(nt,1),z(nt,2)); + x0 = impact_abs_red(xf); + z0 = zd_project_cc(x0(1:10)'); + + + % monitor flag from Grizzle's impact function + if x0(11) + disp('Stepping error from Grizzle impact function.') + end + + step_len = step_length(xf); + + % Diplay some useful info + im = sprintf('%.5f',x0(12)); + dt = sprintf('%.5f',t(length(t))-t(1)); + sl = sprintf('%.5f',step_len); + ra = sprintf('%.5f',step_len/(t(length(t))-t(1))); + disp(['step: ',num2str(i),', impact: ',im, ... + ', delta_t: ',dt,', length: ',sl,', rate: ',ra]); + + step_t = [step_t t(length(t))]; + + % Stop simulation if swing foot is behind stance foot at end of + % "step" + if step_len <= 0 + disp('Stepping error ... swing foot didn''t swing forward.'); + end + end + + tstart = t(nt); + STEP_TIME = t(nt); + if tstart >= tfinal + break + end +end + + +if 0 + xout = zd_lift(zout(:,1)',zout(:,2)').'; + anim(tout,xout,1/30) + + disp('[Done with simulation, about to draw zero dynamics'' plot]'); + + % use this when you run a lot of steps to see the limit cycle + % form in the zero dynamics!! + fig_hl=figure(2); + clf + set(fig_hl,'Position',[50 290 400 400]); + set(fig_hl,'PaperPosition',[1.25 1.5 6 8]) + + vectfield('zd_cc',2.8:0.05:3.5,-2:.2:-.2,.999); + hold on + plot(zout(:,1),zout(:,2)); + xlabel('z_1 \rightarrow \theta_1'); + ylabel('z_2 \rightarrow d\theta_1'); + title('Limit cycle of zero dynamics overlayed on vector field'); + grid; +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/sim_zd_opt.m b/demos/rabbit/anim/rabbit_model/sim_zd_opt.m new file mode 100644 index 0000000..de1a98d --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/sim_zd_opt.m @@ -0,0 +1,92 @@ +function varargout = sim_zd_opt(t,x,flag,opt_param) +%SIM_ZD_OPT Simulate the zero dynamics of the kneed biped walker +% for closed-loop optimization purposes. + +%Eric Westervelt +%3/13/01 + + +switch flag +case '' % Return dx/dt = dynamics(t,x). + varargout{1} = f(t,x); +case 'events' % Return [value,isterminal,direction]. + [varargout{1:3}] = events(t,x); +otherwise + error(['Unknown flag ''' flag '''.']); +end + + +% -------------------------------------------------------------------------- +% This is the system dynamics function + +function dz = f(t,z) + +dz = zd(z); + +% -------------------------------------------------------------------------- +% This is the events function + +function [value,isterminal,direction] = events(t,z) +%Locate the time when critial angle of stance leg minus stance leg +%angle passes through zero in a decreasing direction and stop +%integration. + +global SIM_COUNTER +global T_STOP +global STEP_LEN + +SIM_COUNTER = SIM_COUNTER + 1; + +dz = zd(z); +x = zd_lift(z(1),dz(1)).'; + +if SIM_COUNTER > 200 & isempty(T_STOP) + T_STOP = t; + value(9) = 1; +elseif ~isempty(T_STOP) & abs(rem(t,T_STOP)) < 1e-3 + value(9) = 1; +else + value(9) = 0; +end + +[pHh,pHv]=hip_pos(x); + +h = swing_foot_height(x'); + +value(1) = h+.005; % this will simulate for a longer time + % guaranteeing that the swing foot will contact + % the ground +value(2) = x(5) - pi/3; % torso not too far backward +value(3) = x(5) + pi/3; % torso not too far forward +value(4) = x(2) - 100/180*pi; % swing femur not too far back +value(5) = x(2) - 260/180*pi; % swing femur not too far forward +value(6) = pHv - .6; % hips not too low +value(7) = x(1) - x(3) - 3*pi/4; % stance knee not too bent +value(8) = x(2) - x(4) - 3*pi/4; % swing knee not too bent +value(10) = h; % this is the actual touching of the foot with the + % ground +value(11) = 1.2*STEP_LEN - step_length(x'); + +isterminal(1) = 1; % stop when this event occurs +isterminal(2) = 1; % stop " +isterminal(3) = 1; % stop " +isterminal(4) = 1; % stop " +isterminal(5) = 1; % stop " +isterminal(6) = 1; % stop " +isterminal(7) = 1; % stop " +isterminal(8) = 1; % stop " +isterminal(9) = 1; % stop " +isterminal(10) = 0; +isterminal(11) = 1; + +direction(1) = -1; % decreasing direction +direction(2) = 0; % either +direction(3) = 0; % either +direction(4) = 0; % either +direction(5) = 0; % either +direction(6) = 0; % either +direction(7) = 0; % either +direction(8) = 0; % either +direction(9) = 0; % either +direction(10) = -1; +direction(11) = 0; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/start_of_step_ctrl_sens.m b/demos/rabbit/anim/rabbit_model/start_of_step_ctrl_sens.m new file mode 100644 index 0000000..c3234c6 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/start_of_step_ctrl_sens.m @@ -0,0 +1,147 @@ +function [out,nominal] = start_of_step_ctrl_sens(method) +%START_OF_STEP_CTRL_SENS + +%Eric Westervelt +%2/6/01 +%2/27/01 major update + +%3/23/01 could be a problem with the remove of FIXED_POINT.M and +%upgrade to CONTROLPARAMETERS.M + +global WHICH_TO_TWEAK TWEAK_AMOUNT + +WHICH_TO_TWEAK = []; + +[a,b,m,dtheta_fixed] = controlParameters; + +gains = ones(4,1); + +TWEAK_AMOUNT = 1.000001; + +range = .01; + +a_bar = []; +for WHICH_TO_TWEAK = 1:24 + a_tmp = controlParameters; + a_bar(WHICH_TO_TWEAK) = a_tmp(WHICH_TO_TWEAK+4); +end +WHICH_TO_TWEAK = []; +a = controlParameters; +delta_a = a(5:length(a))-a_bar'; + +for WHICH_TO_TWEAK = 0:24 + + switch method + case 0, % never used + % THIS DOESN'T GIVE MUCH INFORMATION + % + %tmp = inv(LgLfH)*jac_H*(x_plus(6:10)-x_minus(6:10)); + %u_imp(WHICH_TO_TWEAK+1,:) = tmp'; + error('this case not supported'); + case 1, % gradient of initial control signal w.r.t. the control + % parameters + + % nominal impulses + % + x_minus = sigma(dtheta_fixed); + x_plus = impact_abs_red(x_minus); x_plus = x_plus(1:10); + [H,LfH,L2fH,LgLfH] = decouple_abs_red(x_plus(1:5), ... + x_plus(6:10)); + + % the nomimal initial control + % + ctrl_out = [bb([H(1),LfH(1)],gains(1)); + bb([H(2),LfH(2)],gains(2)); + bb([H(3),LfH(3)],gains(3)); + bb([H(4),LfH(4)],gains(4))]; + psi = ctrl_out(:,1); + tmp = inv(LgLfH)*(psi-L2fH); + u_init(WHICH_TO_TWEAK+1,:) = tmp'; + + % the nomimal initial control for state on the zd + % + z = zd_project_cc(x_plus); + x_plus_zd = zd_lift(z(1),z(2)); + [H,LfH,L2fH,LgLfH] = decouple_abs_red(x_plus_zd(1:5), ... + x_plus_zd(6:10)); + + ctrl_out = [bb([H(1),LfH(1)],gains(1)); + bb([H(2),LfH(2)],gains(2)); + bb([H(3),LfH(3)],gains(3)); + bb([H(4),LfH(4)],gains(4))]; + psi = ctrl_out(:,1); + tmp = inv(LgLfH)*(psi-L2fH); + u_init_zd(WHICH_TO_TWEAK+1,:) = tmp'; + + if WHICH_TO_TWEAK == 0, + nominal = norm(u_init(1,:) - u_init_zd(1,:)); + else + tmp = norm(u_init(WHICH_TO_TWEAK+1,:) - ... + u_init_zd(WHICH_TO_TWEAK+1,:)); + out(WHICH_TO_TWEAK) = (tmp - nominal)/ ... + delta_a(WHICH_TO_TWEAK); + end + case 2, % approximate the gradient of the integral of LfH around + % the fixed point on the zero dynamics w.r.t. the + % control parameters + + x_minus = sigma(dtheta_fixed); + q = x_minus(1:5); + + % point at fixed point + x_plus = impact_abs_red(x_minus); + [H,LfH] = decouple_abs_red(x_plus(1:5),x_plus(6:10)); + tmp = norm(LfH)*range; + + % take two points on either side of fixed point + for k = 1:2 + % points below + dq = sigma_vel(dtheta_fixed - k*range); + x_plus = impact_abs_red([q;dq]); + [H,LfH] = decouple_abs_red(x_plus(1:5),x_plus(6:10)); + tmp = tmp + norm(LfH)*range; + + %points above + dq = sigma_vel(dtheta_fixed + k*range); + x_plus = impact_abs_red([q;dq]); + [H,LfH] = decouple_abs_red(x_plus(1:5),x_plus(6:10)); + tmp = tmp + norm(LfH)*range; + end + + if WHICH_TO_TWEAK == 0 % first case is nominal + nominal = tmp; + else % second case are the control parameter perturbations + out(WHICH_TO_TWEAK) = (tmp - nominal)/ ... + delta_a(WHICH_TO_TWEAK); + end + + otherwise + error('METHOD parameter out of range'); + end +end + +% print this stuff out for documenting work +% +disp(' '); +disp(['Tweak amount = ',num2str(TWEAK_AMOUNT)]); + +if 1 + disp(['nominal = ',num2str(nominal,'%.8f')]); + disp(' '); + + for k = 1:4 + switch k + case 1, chr = 'a'; + case 2, chr = 'b'; + case 3, chr = 'c'; + case 4, chr = 'd'; + end + + str = []; + for m = 1:5 + str = [str,num2str(out((k-1)*6+m),'%.8f'),', ']; + end + str = [str,num2str(out(k*6),'%.8f')]; + disp([chr,' = [',str,']']); + end +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/step_length.m b/demos/rabbit/anim/rabbit_model/step_length.m new file mode 100644 index 0000000..e27c994 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/step_length.m @@ -0,0 +1,11 @@ +function h = step_length(x) +% STEP_LENGTH The step length. +% H = STEP_LENGTH(X) + +%Eric Westervelt +%24-Mar-2001 10:46:13 + +[g,L1,L3,L4] = modelParameters; + +q31=x(1); q32=x(2); q41=x(3); q42=x(4); q1=x(5); +h = L3*sin(q31)+L4*sin(q41)-L3*sin(q32)-L4*sin(q42); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/swing_foot_height.m b/demos/rabbit/anim/rabbit_model/swing_foot_height.m new file mode 100644 index 0000000..4407984 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/swing_foot_height.m @@ -0,0 +1,16 @@ +function h = swing_foot_height(x) +% SWING_FOOT_HEIGHT The swing foot height. +% H = SWING_FOOT_HEIGHT(X) + +%Eric Westervelt +%24-Mar-2001 10:46:12 + +[g,L1,L3,L4] = modelParameters; + +[n,m] = size(x); + +if n ~= 1 & m ~= 1 + h = -L3.*cos(x(:,1))-L4.*cos(x(:,3))+L3.*cos(x(:,2))+L4.*cos(x(:,4)); +else + h = -L3*cos(x(1))-L4*cos(x(3))+L3*cos(x(2))+L4*cos(x(4)); +end diff --git a/demos/rabbit/anim/rabbit_model/swing_foot_velocity.m b/demos/rabbit/anim/rabbit_model/swing_foot_velocity.m new file mode 100644 index 0000000..58c1a8b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/swing_foot_velocity.m @@ -0,0 +1,18 @@ +function [vh,vv] = swing_foot_velocity(x) +% SWING_FOOT_VELOCITY The swing foot vertical velocity. +% [VH,VV] = SWING_FOOT_VELOCITY(X) + +%Eric Westervelt +%24-Mar-2001 10:46:13 + +[g,L1,L3,L4] = modelParameters; + +[n,m] = size(x); + +if n ~= 1 & m ~= 1 + vh = L3.*cos(x(:,1)).*x(:,6)-L3.*cos(x(:,2)).*x(:,7)+L4.*cos(x(:,3)).*x(:,8)-L4.*cos(x(:,4)).*x(:,9); + vv = L3.*sin(x(:,1)).*x(:,6)-L3.*sin(x(:,2)).*x(:,7)+L4.*sin(x(:,3)).*x(:,8)-L4.*sin(x(:,4)).*x(:,9); +else + vh = L3*cos(x(1))*x(6)-L3*cos(x(2))*x(7)+L4*cos(x(3))*x(8)-L4*cos(x(4))*x(9); + vv = L3*sin(x(1))*x(6)-L3*sin(x(2))*x(7)+L4*sin(x(3))*x(8)-L4*sin(x(4))*x(9); +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/symb_control.m b/demos/rabbit/anim/rabbit_model/symb_control.m new file mode 100644 index 0000000..08b4691 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/symb_control.m @@ -0,0 +1,377 @@ +%SYMB_CONTROL.M + +%Eric Westervelt +%2/15/01 + +clear + +if isempty(dir('mat_files/work_symb_control_abs_red_test.mat')) + load mat_files/work_symb_model_abs_red + + which_one = 1; + switch which_one + case 1 % using bezier curves + syms u theta1 dtheta1 real + + syms a0 a1 a2 a3 a4 a5 a6 + syms b0 b1 b2 b3 b4 b5 b6 + syms c0 c1 c2 c3 c4 c5 c6 + syms d0 d1 d2 d3 d4 d5 d6 + + a = [a0 a1 a2 a3 a4 a5 a6]; + b = [b0 b1 b2 b3 b4 b5 b6]; + c = [c0 c1 c2 c3 c4 c5 c6]; + d = [d0 d1 d2 d3 d4 d5 d6]; + + p1 = 0; + p2 = 0; + p3 = 0; + p4 = 0; + + % form bezier parameterization of polynomials + NN = 6; + for k = 1:NN+1 + kk = k - 1; + p1 = p1 + a(k)*factorial(NN)/ ... + factorial(kk)/factorial(NN-kk)*u^(kk)*(1-u)^(NN-kk); + p2 = p2 + b(k)*factorial(NN)/ ... + factorial(kk)/factorial(NN-kk)*u^(kk)*(1-u)^(NN-kk); + p3 = p3 + c(k)*factorial(NN)/ ... + factorial(kk)/factorial(NN-kk)*u^(kk)*(1-u)^(NN-kk); + p4 = p4 + d(k)*factorial(NN)/ ... + factorial(kk)/factorial(NN-kk)*u^(kk)*(1-u)^(NN-kk); + end + + syms b m + + p1 = subs(p1,u,(theta1-b)/m); + p2 = subs(p2,u,(theta1-b)/m); + p3 = subs(p3,u,(theta1-b)/m); + p4 = subs(p4,u,(theta1-b)/m); + + A = [ 0 0 0 0 1; + -1 1 0 0 0; + -1 0 1 0 0; + 0 -1 0 1 0]; + + P = [p1; + p2; + p3; + p4]; + + % The output function: + % + % a linear term, constant term, plus a nonlinear term + % parameterized by theta + % + % A and P implement + % + % h = [q1 - p1; + % q32-q31 - p2; + % q41-q31 - p3; + % q42-q32 - p4]; + % + h = A*q-P; + Ah = [A; 1/2 0 1/2 0 0]; + + Ah_inv = inv(Ah); + + %syms xh1 xh2 xh3 xh4 xh5 + %xh = [xh1; xh2; xh3; xh4; xh5]; + + %syms xh6 xh7 xh8 xh9 xh10 + %dxh = [xh6; xh7; xh8; xh9; xh10]; + + q_sigma_zd = Ah_inv*[P;theta1]; + dq_sigma_zd = Ah_inv*[jacobian(P, theta1)*dtheta1;dtheta1]; + + %q_sigma_zd = subs(q_sigma, ... + %{xh1,xh2,xh3,xh4,xh5,xh6,xh7,xh8,xh9,xh10}, ... + %{0,0,0,0,theta1,0,0,0,0,dtheta1},0); + %dq_sigma_zd = subs(dq_sigma, ... + %{xh1,xh2,xh3,xh4,xh5,xh6,xh7,xh8,xh9,xh10}, ... + %{0,0,0,0,theta1,0,0,0,0,dtheta1},0); + + %q32_sigma = simple(subs(q_sigma_zd(2),theta1,1/2*(q31+q41))); + %q42_sigma = simple(subs(q_sigma_zd(4),theta1,1/2*(q31+q41))); + %q1_sigma = q_sigma_zd(5); + + dq31_sigma = dq_sigma_zd(1); + dq32_sigma = dq_sigma_zd(2); + dq41_sigma = dq_sigma_zd(3); + dq42_sigma = dq_sigma_zd(4); + dq1_sigma = dq_sigma_zd(5); + + syms aN bN cN dN + q_end = Ah_inv*[aN; bN; cN; dN; theta1]; + + obj_fun_foot_on_ground = subs(pFoot2(2),{q31,q32,q41,q42},... + {q_end(1),q_end(2),q_end(3),q_end(4)},0); + end + + h = subs(h,theta1,1/2*(q31+q41)); + + diffeo = [h; 1/2*(q31+q41)]; + + jac_h = jacobian(h,q); % y_dot = jac_h * dq + + % the next command only made things worse + % jac_h = simple(jac_h); + lfh = jac_h*dq; + + gamma = jacobian(jac_h*dq,q)*dq; % y_ddot = gamma + jac_h * ddq + + % the next command only made things worse + % gamma = simple(gamma); + + save mat_files/work_symb_control_abs_red_test +else + load mat_files/work_symb_control_abs_red_test + disp('[DONE loading work_symb_control_abs_red.mat]') +end + +%------------------------------------------------------ +% +% Automatically generate .m-files needed for simulation +% +fcn_name = 'decouple_abs_red'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +n=max(size(q)); +fprintf(fid,['function [h,lfh,l2fh,lglfh,jac_h,inv_D,C,G,B]=' ... + ' %s(q,dq)\n'],fcn_name); +fprintf(fid,'%%%s\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); +fprintf(fid,'[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]='); +fprintf(fid,' modelParameters;\n\n'); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +a = char('a'); +for j = 0:3 + for k = 0:NN + fprintf(fid,[char(a+j),num2str(k),'=a(',num2str(1+k+j*(NN+1)),'); ']); + end + fprintf(fid,'\n'); +end +fprintf(fid,'\n'); + +fprintf(fid,'q31=q(1);q32=q(2);q41=q(3);q42=q(4);q1=q(5);'); +fprintf(fid,'\ndq31=dq(1);dq32=dq(2);dq41=dq(3);'); +fprintf(fid,'dq42=dq(4);dq1=dq(5);'); +fprintf(fid,'\n\ntheta1=1/2*(q31+q41);\n'); +p=max(size(h)); +fprintf(fid,'\n%s',['h=zeros(',num2str(p),',1);']); +for i=1:p + Temp1=char(h(i)); + Temp2=['h(',num2str(i),')=',Temp1,';']; + fprintf(fid,'\n%s',Temp2); +end +fprintf(fid,'\n%s','%%'); +fprintf(fid,'\n%s','%%'); +fprintf(fid,'\n%s','%%'); +p=max(size(lfh)); +fprintf(fid,'\n%s',['lfh=zeros(',num2str(p),',1);']); +for i=1:p + Temp1=char(lfh(i)); + Temp2=['lfh(',num2str(i),')=',Temp1,';']; + fprintf(fid,'\n%s',Temp2); +end +fprintf(fid,'\n%%'); +fprintf(fid,'\n%%'); +fprintf(fid,'\n%%'); +[p,n]=size(jac_h); +fprintf(fid,'\n%s',['jac_h=zeros(',num2str(p),',',num2str(n),');']); +for i=1:p + for j=1:n + Temp0=jac_h(i,j); + if Temp0 ~= 0 + Temp1=char(Temp0); + Temp2=['jac_h(',num2str(i),',',num2str(j),')=',Temp1,';']; + fprintf(fid,'\n%s',Temp2); + end + end +end +fprintf(fid,'\n%%'); +fprintf(fid,'\n%%'); +fprintf(fid,'\n%%'); +p=max(size(gamma)); +fprintf(fid,'\n%s',['gamma=zeros(',num2str(p),',1);']); +for i=1:p + Temp1=char(gamma(i)); + Temp2=['gamma(',num2str(i),')=',Temp1,';']; + fprintf(fid,'\n%s',Temp2); +end +fprintf(fid,'\n%%'); +fprintf(fid,'\n%%'); +fprintf(fid,'\n[D,C,G,B]=dyn_mod_abs_red([q;dq]);'); +fprintf(fid,'\ninv_D=inv(D);'); +fprintf(fid,'\nl2fh=gamma-jac_h*inv_D*(C*dq+G);'); +fprintf(fid,'\nlglfh=jac_h*inv_D*B;'); +fclose(fid); +% +% +% +fcn_name = 'sigma'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function x0 = %s(dtheta1,theta1)\n',fcn_name); +fprintf(fid,['%%%s Maps horizontal velocity of hips just before' ... + ' impact to\n'],upper(fcn_name)); +fprintf(fid,'%% state of the system just before impact.\n'); +fprintf(fid,'%%\n%% X0 = %s(DTHETA1,THETA1)\n\n', ... + upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +fprintf(fid,'Ah_inv = zeros(5);\n'); +for k=1:5, + for j=1:5, + if Ah_inv(k,j)~=0 + ttt=num2str(Ah_inv(k,j)); + fprintf(fid,'Ah_inv(%s,%s)=%s;\n',num2str(k), ... + num2str(j),ttt); + end + end +end +fprintf(fid,['\naN = a([',num2str(1+NN),';', ... + num2str(1+2*(NN+1)-1),';', ... + num2str(1+3*(NN+1)-1),';', ... + num2str(1+4*(NN+1)-1),']);\n']); +fprintf(fid,['aN_minus_1 = a([',num2str(1+NN-1),';', ... + num2str(1+2*(NN+1)-2),';', ... + num2str(1+3*(NN+1)-2),';', ... + num2str(1+4*(NN+1)-2),']);\n\n']); +fprintf(fid,'if nargin == 2\n'); +fprintf(fid,' q = Ah_inv*[aN; theta1];\n'); +fprintf(fid,'else\n'); +fprintf(fid,' q = Ah_inv*[aN; b+m];\n'); +fprintf(fid,'end\n\n'); +fprintf(fid,['dq = Ah_inv*[-',num2str(NN),'*(aN_minus_1-aN)/m;' ... + ' 1]*dtheta1;\n\n']); +fprintf(fid,'x0 = [q; dq];'); +fclose(fid); +% +% +% +fcn_name = 'theta_obj'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function f = %s(theta1)\n',fcn_name); +fprintf(fid,['%%%s Function to find theta.\n%%\n'], ... + upper(fcn_name)); +fprintf(fid,'%% F = %s(THETA1)\n',fcn_name); +fprintf(fid,'\n'); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'a = controlParameters;\n\n'); + +fprintf(fid,['aN=a(',num2str(1+NN),'); ', ... + 'bN=a(',num2str(1+2*(NN+1)-1),'); ', ... + 'cN=a(',num2str(1+3*(NN+1)-1),'); ', ... + 'dN=a(',num2str(1+4*(NN+1)-1),');\n\n']); + +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); +ttt=fixlength(char(vectorize(obj_fun_foot_on_ground)), ... + '*+-',65,' '); +fprintf(fid,'f = %s;',ttt); +fclose(fid); +% +% +% +fcn_name = 'sigma_vel'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function dq = %s(dtheta1)\n',fcn_name); +fprintf(fid,['%%%s Maps horizontal velocity of hips just before' ... + ' impact to\n'],upper(fcn_name)); +fprintf(fid,'%% state of the system just before impact.\n'); +fprintf(fid,'%%\n%% DQ = %s(DTHETA1)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +fprintf(fid,'Ah_inv = zeros(5);\n'); +for k=1:5, + for j=1:5, + if Ah_inv(k,j)~=0 + ttt=num2str(Ah_inv(k,j)); + fprintf(fid,'Ah_inv(%s,%s)=%s;\n',num2str(k), ... + num2str(j),ttt); + end + end +end +fprintf(fid,['\naN = a([',num2str(1+NN),';', ... + num2str(1+2*(NN+1)-1),';', ... + num2str(1+3*(NN+1)-1),';', ... + num2str(1+4*(NN+1)-1),']);\n']); +fprintf(fid,['aN_minus_1 = a([',num2str(1+NN-1),';', ... + num2str(1+2*(NN+1)-2),';', ... + num2str(1+3*(NN+1)-2),';', ... + num2str(1+4*(NN+1)-2),']);\n\n']); +fprintf(fid,['dq = Ah_inv*[-',num2str(NN),'*(aN_minus_1-aN)/m;' ... + ' 1]*dtheta1;']); +fclose(fid); +% +% +% +fcn_name = 'zd_lift'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function x = %s(theta1,dtheta1)\n',fcn_name); +fprintf(fid,['%%%s Lifts the zero dynamics to the full state of' ... + ' the system.\n'],upper(fcn_name)); +fprintf(fid,'%%\n%% X = %s(THETA1,DTHETA1)\n\n', ... + upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + +a = char('a'); +for j = 0:3 + for k = 0:NN + fprintf(fid,[char(a+j),num2str(k),'=a(',num2str(1+k+j*(NN+1)),'); ']); + end + fprintf(fid,'\n'); +end +fprintf(fid,'\n'); + +ttt = char(vectorize(q_sigma_zd(1))); +fprintf(fid,'q31 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(q_sigma_zd(2))); +fprintf(fid,'q32 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(q_sigma_zd(3))); +fprintf(fid,'q41 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(q_sigma_zd(4))); +fprintf(fid,'q42 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(q_sigma_zd(5))); +fprintf(fid,'q1 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(dq31_sigma)); +fprintf(fid,'dq31 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(dq32_sigma)); +fprintf(fid,'dq32 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(dq41_sigma)); +fprintf(fid,'dq41 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(dq42_sigma)); +fprintf(fid,'dq42 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +ttt = char(vectorize(dq1_sigma)); +fprintf(fid,'dq1 = %s;\n\n',fixlength(ttt,'*+-',65,' ')); + +fprintf(fid,['x = [q31; q32; q41; q42; q1; dq31; dq32; dq41; dq42;' ... + ' dq1];']); +fclose(fid); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/symb_model.m b/demos/rabbit/anim/rabbit_model/symb_model.m new file mode 100644 index 0000000..cf29f50 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/symb_model.m @@ -0,0 +1,367 @@ +%SYMB_MODEL.M + +%From J.W. Grizzle and F. Plestan +%2/15/01 + +clear + +% This is for a robot with an upright trunk, two legs and knees. The model is +% for five degrees of freedom of the robot, with Foot1 touching the ground +% +syms q1 q31 q32 q41 q42 y z dq1 dq31 dq32 dq41 dq42 dy dz real +syms g L1 L3 L4 M1 M3 M4 real +syms MY1 MZ1 MZ3 MZ4 XX1 XX3 XX4 real +% +% reference = absolute angles for everything; trigonometric sense for all +% angles, measured from the vertical, +% +q=[q31 q32 q41 q42 y z q1].'; +dq=[dq31 dq32 dq41 dq42 dy dz dq1].'; +% +% +pH=[y;z]; +pG1 = pH + L3*[-sin(q31);cos(q31)]; +pG2 = pH + L3*[-sin(q32);cos(q32)]; +% +% +vH = jacobian(pH,q)*dq; +vG1 = jacobian(pG1,q)*dq; +vG2 = jacobian(pG2,q)*dq; +% +% +R_T=[cos(q1) -sin(q1); sin(q1) cos(q1)]; +vH_RT = R_T.'*vH; +R_Fem1=[cos(q31) -sin(q31); sin(q31) cos(q31)]; +vH_Fem1 = R_Fem1.'*vH; +R_Fem2=[cos(q32) -sin(q32); sin(q32) cos(q32)]; +vH_Fem2 = R_Fem2.'*vH; +R_Tib1=[cos(q41) -sin(q41); sin(q41) cos(q41)]; +vG1_Tib1 = R_Tib1.'*vG1; +R_Tib2=[cos(q42) -sin(q42); sin(q42) cos(q42)]; +vG2_Tib2 = R_Tib2.'*vG2; +% +% +KET = simplify((1/2)*M1*vH.'*vH + vH_RT.'*[-MZ1;MY1]*dq1 ... + +(1/2)*XX1*dq1^2); +KEFem1 = simplify((1/2)*M3*vH.'*vH + vH_Fem1.'*[-MZ3;0]*(dq31) ... + +(1/2)*XX3*(dq31)^2); +KEFem2 = simplify((1/2)*M3*vH.'*vH + vH_Fem2.'*[-MZ3;0]*(dq32) ... + +(1/2)*XX3*(dq32)^2); +KETib1 = simplify((1/2)*M4*vG1.'*vG1 + vG1_Tib1.'*[-MZ4;0]*(dq41) ... + +(1/2)*XX4*(dq41)^2); +KETib2 = simplify((1/2)*M4*vG2.'*vG2 + vG2_Tib2.'*[-MZ4;0]*(dq42) ... + +(1/2)*XX4*(dq42)^2); +% +% +KE = simplify(KET + KEFem1 + KEFem2 + KETib1 + KETib2); +KE=simple(KE); +% +% +% centers of gravity and positions of the various members +% +%pT = pH + (1/M1)*(MY1^2+MZ1^2)^(.5)*[-sin(q1-atan(MY1/MZ1));cos(q1-atan(MY1/MZ1))]; +pT = pH + (1/M1)*[-(sin(q1)*MZ1-cos(q1)*MY1);(cos(q1)*MZ1+sin(q1)*MY1)]; +pFem1 = pH + (MZ3/M3)*[-sin(q31);cos(q31)]; +pFem2 = pH + (MZ3/M3)*[-sin(q32);cos(q32)]; +pG1 = pH +[-L3*sin(q31);L3*cos(q31)]; +pG2 = pH +[-L3*sin(q32);L3*cos(q32)]; +pTib1 = pG1 + [-(MZ4/M4)*sin(q41);(MZ4/M4)*cos(q41)]; +pTib2 = pG2 + [-(MZ4/M4)*sin(q42);(MZ4/M4)*cos(q42)]; +pFoot1= pG1 + [-L4*sin(q41);L4*cos(q41)]; +pFoot2= pG2 + [-L4*sin(q42);L4*cos(q42)]; +% +% +% +vFoot1=jacobian(pFoot1,q)*dq; +vT=jacobian(pT,q)*dq; +% +% +PE = g*(M1*pT(2) + M3*pFem1(2) + M3*pFem2(2) + M4*pTib1(2) + M4*pTib2(2)); +PE=simple(PE); +% +% +% Model NOTATION: Spong and Vidyasagar, page 142, Eq. (6.3.12) +% D(q)ddq + C(q,dq)*dq + G(q) = B*tau + E2*F_external +% +L=KE-PE; +%save mat_files/work_symb_model_abs +%return +G=jacobian(PE,q).'; +G=simple(G); +D=simple(jacobian(KE,dq).'); +D=simple(jacobian(D,dq)); +%C=jacobian(L,dq).'; +%C=jacobian(C,q)-(1/2)*jacobian(D*dq,q).'; +%C=simple(C); +%temp1=C*dq+G; +%temp2=jacobian(jacobian(L,dq).',q)*dq-jacobian(L,q).'; +%check=temp1-temp2; +%check=simple(check); +%check +%save mat_files/work_symb_model_abs +%return +syms C real +n=max(size(q)); +for k=1:n + for j=1:n + C(k,j)=0*g; + for i=1:n + C(k,j)=C(k,j)+(1/2)*(diff(D(k,j),q(i))+diff(D(k,i),q(j))-diff(D(i,j),q(k)))*dq(i); + end + end +end +C=simple(C); +% +% Compute the matrix for the input torques and then the contact points. +% +Phi_0=[q31-q1;q32-q1;q41-q31;q42-q32;y;z;q1]; +B=jacobian(Phi_0,q); +B=B.'*[eye(4,4);zeros(3,4)]; +% +% +% F_ext = [F_T^1;F_N^1;F_T^2;F_N^2]; +% +Phi_1=[pFoot1;pFoot2]; %HERE why is pFoot1 HERE? +E=jacobian(Phi_1,q); +E=E.'; + +save mat_files/work_symb_model_abs + +if 0 + % + % For later use, compute D^{-1} in the form of DI/det(D); + % + d=simple(det(D)); + DI=inv(D); + DI=simplify(d*DI); + DI=simple(DI); + save mat_files/work_symb_model_abs_red +end + +N=max(size(q)); + +%% First, output model to a file called +%%%% dyn_mod_abs.m +%% +% + +%% write file header +% +fcn_name = 'dyn_mod_abs'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function [D,C,G,B,E]=%s(x)\n',fcn_name); +fprintf(fid,['%% %s Grizzle''s full model of kneed biped walker' ... + ' model.\n'],upper(fcn_name)); + +fprintf(fid,'%% [DE,CE,GE,BE,EE] = %s',upper(fcn_name)); +fprintf(fid,'(X) is the kneed\n'); +fprintf(fid,'%% biped walking model. (x is of dimension %s)\n',... + num2str(2*N)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +%% Read in constants +% +fprintf(fid,['[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=' ... + ' modelParameters;\n\n']); + +%% Reassign configuration parameters +% +fprintf(fid,'q31=x(1); q32=x(2); q41=x(3); q42=x(4);'); +fprintf(fid,' y=x(5); z=x(6); q1=x(7);\n'); +fprintf(fid,'dq31=x(8); dq32=x(9); dq41=x(10); dq42=x(11);'); +fprintf(fid,' dy=x(12); dz=x(13); dq1=x(14);\n\n'); + +%% Model output +% +fprintf(fid,'%% D matrix\n'); +fprintf(fid,'D=zeros(%s);\n',num2str(N)); +for k=1:N, + for j=1:N, + if D(k,j)~=0 + ttt=char(D(k,j)); + fprintf(fid,'D(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end + +fprintf(fid,'\n%% C matrix\n'); +fprintf(fid,'C=zeros(%s);\n',num2str(N)); +for k=1:N, + for j=1:N, + if C(k,j)~=0 + ttt=char(C(k,j)); + fprintf(fid,'C(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end + +fprintf(fid,'\n%% G matrix\n'); +fprintf(fid,'G=zeros(%s,1);\n',num2str(N)); +for k=1:N, + if G(k)~=0 + ttt=char(G(k)); + fprintf(fid,'G(%s)=%s;\n',num2str(k),ttt); + end +end + +fprintf(fid,'\n%% B matrix\n'); +[N,M]=size(B); +fprintf(fid,'B=zeros(%s,%s);\n',num2str(N),num2str(M)); +for k=1:N, + for j=1:M, + if B(k,j)~=0 + ttt=char(B(k,j)); + fprintf(fid,'B(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end + +fprintf(fid,'\n%% E matrix\n'); +[N,M]=size(E); +fprintf(fid,'E=zeros(%s,%s);\n',num2str(N),num2str(M)); +for k=1:N, + for j=1:M, + if E(k,j)~=0 + ttt=char(E(k,j)); + fprintf(fid,'E(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end + +fclose(fid); +% +% +% +if 0 + %% write file header + % + fcn_name = 'stance_force'; + disp(['[creating ',upper(fcn_name),'.m]']); + fid = fopen([fcn_name,'.m'],'w'); + fprintf(fid,'function [f_tan,f_norm] = %s(x,dx,u)\n',fcn_name); + fprintf(fid,['%% %s Calculated the forces on the stance leg during' ... + ' impact.\n'],upper(fcn_name)); + fprintf(fid,['%% [F_TAN,F_NORM] = %s(X,DX,U) are the forces on' ... + ' the stance leg at impact.\n\n'],upper(fcn_name)); + fprintf(fid,'%%Eric Westervelt\n'); + fprintf(fid,'%%%s\n\n',datestr(now)); + + N=max(size(q)); + N_F=max(size(q_F)); + + %% Read in constants + % + fprintf(fid,['[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=' ... + ' modelParameters;\n\n']); + fprintf(fid,'[a,b,m] = controlParameters;\n\n'); + + %% Reassign configuration parameters + % + fprintf(fid,['q31=x(1); q32=x(2); q41=x(3); q42=x(4); q1=x(5);\' ... + ' n']); + + fprintf(fid,['dq31=x(6); dq32=x(7); dq41=x(8); dq42=x(9); dq1=' ... + ' x(10);\n']); + + %% Model output + % + fprintf(fid,'%% D_Fs11 matrix\n'); + fprintf(fid,'D_Fs11=zeros(%s,%s);\n',num2str(N),num2str(N)); + for k=1:N, + for j=1:N, + if D_Fs(k,j)~=0 + ttt=char(D_Fs(k,j)); + fprintf(fid,'D_Fs11(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end + end + + fprintf(fid,'\n%% D_Fs12 matrix\n'); + fprintf(fid,'D_Fs12=zeros(%s,%s);\n',num2str(N),num2str(N_F-N)); + for k=1:N, + for j=1:N_F-N, + if D_Fs(k,j+N)~=0 + ttt=char(D_Fs(k,j+N)); + fprintf(fid,'D_Fs12(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end + end + + fprintf(fid,'\n%% D_Fs22 matrix\n'); + fprintf(fid,'D_Fs22=zeros(%s,%s);\n',num2str(N_F-N),num2str(N_F-N)); + for k=1:N_F-N, + for j=1:N_F-N, + if D_Fs(k+N,j+N)~=0 + ttt=char(D_Fs(k+N,j+N)); + fprintf(fid,'D_Fs22(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end + end + + fprintf(fid,'\n%% C_Fs11 matrix\n'); + fprintf(fid,'C_Fs11=zeros(%s,%s);\n',num2str(N),num2str(N)); + for k=1:N, + for j=1:N, + if C_Fs(k,j)~=0 + ttt=char(C_Fs(k,j)); + fprintf(fid,'C_Fs11(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end + end + + fprintf(fid,'\n%% C_Fs21 matrix\n'); + fprintf(fid,'C_Fs21=zeros(%s,%s);\n',num2str(N_F-N),num2str(N)); + for k=1:N_F-N, + for j=1:N, + if C_Fs(k+N,j)~=0 + ttt=char(C_Fs(k+N,j)); + fprintf(fid,'C_Fs21(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end + end + + fprintf(fid,'\n%% G_Fs1 matrix\n'); + fprintf(fid,'G_Fs1=zeros(%s,%s);\n',num2str(N),num2str(1)); + for k=1:N, + if G_Fs(k)~=0 + ttt=char(G_Fs(k)); + fprintf(fid,'G_Fs1(%s,%s)=%s;\n',num2str(k),num2str(1),ttt); + end + end + + fprintf(fid,'\n%% G_Fs2 matrix\n'); + fprintf(fid,'G_Fs2=zeros(%s,%s);\n',num2str(N_F-N),num2str(1)); + for k=1:N_F-N, + if G_Fs(k+N)~=0 + ttt=char(G_Fs(k+N)); + fprintf(fid,'G_Fs2(%s,%s)=%s;\n',num2str(k),num2str(1),ttt); + end + end + + fprintf(fid,'\n%% B matrix\n'); + [N,M]=size(B); + fprintf(fid,'B=zeros(%s,%s);\n',num2str(N),num2str(M)); + for k=1:N, + for j=1:M, + if B(k,j)~=0 + ttt=char(B(k,j)); + fprintf(fid,'B(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end + end + + fprintf(fid,'\n%% See my notes, 9/13/200 for equations...\n'); + + fprintf(fid,'DD=inv((D_Fs12*inv(D_Fs22)).''*D_Fs12...\n'); + fprintf(fid,' *inv(D_Fs22))*(D_Fs12*inv(D_Fs22)).'';\n'); + + fprintf(fid,'F=DD*(-(D_Fs11-D_Fs12*inv(D_Fs22)*D_Fs12.'')...\n'); + fprintf(fid,' *dx(6:10)+(D_Fs12*inv(D_Fs22)*C_Fs21-C_Fs11)...\n'); + fprintf(fid,' *dx(1:5)+D_Fs12*inv(D_Fs22)*G_Fs2-G_Fs1+B*u);\n\n'); + + fprintf(fid,'f_tan=F(1);\n'); + fprintf(fid,'f_norm=F(2);\n'); + fclose(fid); +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/symb_model_red.m b/demos/rabbit/anim/rabbit_model/symb_model_red.m new file mode 100644 index 0000000..5e324fb --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/symb_model_red.m @@ -0,0 +1,638 @@ +function symb_model_red +%SYMB_MODEL_RED.M + +%From J.W. Grizzle and F. Plestan +%2/15/01 + +clear + +% This is for a robot with an upright trunk, two legs and knees. The +% model is for five degrees of freedom of the robot, with Foot1 +% touching the ground +% +syms q1 q31 q32 q41 q42 dq1 dq31 dq32 dq41 dq42 real +syms g L1 L3 L4 M1 M3 M4 real +syms MY1 MZ1 MZ3 MZ4 XX1 XX3 XX4 real +% +% reference = absolute angles for everything; trigonometric sense for +% all angles, measured from the vertical, +% +q=[q31 q32 q41 q42 q1].'; +dq=[dq31 dq32 dq41 dq42 dq1].'; +% +% +pH = -(L3*[-sin(q31); cos(q31)] + L4*[-sin(q41); cos(q41)]); +pG1 = pH + L3*[-sin(q31); cos(q31)]; +pG2 = pH + L3*[-sin(q32); cos(q32)]; +% +% +vH = jacobian(pH,q)*dq; +vG1 = jacobian(pG1,q)*dq; +vG2 = jacobian(pG2,q)*dq; +% +% +R_T=[cos(q1) -sin(q1); sin(q1) cos(q1)]; +vH_RT = R_T.'*vH; +R_Fem1=[cos(q31) -sin(q31); sin(q31) cos(q31)]; +vH_Fem1 = R_Fem1.'*vH; +R_Fem2=[cos(q32) -sin(q32); sin(q32) cos(q32)]; +vH_Fem2 = R_Fem2.'*vH; +R_Tib1=[cos(q41) -sin(q41); sin(q41) cos(q41)]; +vG1_Tib1 = R_Tib1.'*vG1; +R_Tib2=[cos(q42) -sin(q42); sin(q42) cos(q42)]; +vG2_Tib2 = R_Tib2.'*vG2; +% +% +KET = simplify((1/2)*M1*vH.'*vH + vH_RT.'*[-MZ1;MY1]*dq1 + ... + (1/2)*XX1*dq1^2); +KEFem1 = simplify((1/2)*M3*vH.'*vH + vH_Fem1.'*[-MZ3;0]*(dq31) + ... + (1/2)*XX3*(dq31)^2); +KEFem2 = simplify((1/2)*M3*vH.'*vH + vH_Fem2.'*[-MZ3;0]*(dq32) + ... + (1/2)*XX3*(dq32)^2); +KETib1 = simplify((1/2)*M4*vG1.'*vG1 + ... + vG1_Tib1.'*[-MZ4;0]*(dq41) + (1/2)*XX4*(dq41)^2); +KETib2 = simplify((1/2)*M4*vG2.'*vG2 + ... + vG2_Tib2.'*[-MZ4;0]*(dq42) +(1/2)*XX4*(dq42)^2); +% +% +KE = simplify(KET + KEFem1 + KEFem2 + KETib1 + KETib2); +KE=simple(KE); +% +% centers of gravity and positions of the various members +% +pT = pH + (1/M1)*[-(sin(q1)*MZ1-cos(q1)*MY1); + (cos(q1)*MZ1+sin(q1)*MY1)]; +pHead = pH + L1*[-sin(q1); cos(q1)]; +pFem1 = pH + (MZ3/M3)*[-sin(q31);cos(q31)]; +pFem2 = pH + (MZ3/M3)*[-sin(q32);cos(q32)]; +pTib1 = pG1 + (MZ4/M4)*[-sin(q41); cos(q41)]; +pTib2 = pG2 + (MZ4/M4)*[-sin(q42); cos(q42)]; +pFoot1= pG1 + L4*[-sin(q41); cos(q41)]; +pFoot2= pG2 + L4*[-sin(q42); cos(q42)]; +% +% +vFoot1=jacobian(pFoot1,q)*dq; +vFoot2=jacobian(pFoot2,q)*dq; +vT=jacobian(pT,q)*dq; +% +% +PE = g*(M1*pT(2) + M3*pFem1(2) + M3*pFem2(2) + M4*pTib1(2) + ... + M4*pTib2(2)); +PE = simple(PE); +% +% +% Center of mass calculation...may want to use this for the theta1 +% parameter to watch the center of mass trajectory (see notes +% 3/5/01 or Principles of Dynamics by Greenwood, page 136) +% +% +M_total = M1 + 2*M3 + 2*M4; +cm = 1/M_total*(M1*pT + M3*pFem1 + M3*pFem2 + M4*pTib1 + M4*pTib2); +cm = simple(cm); +% +% +% Model NOTATION: Spong and Vidyasagar, page 142, Eq. (6.3.12) +% D(q)ddq + C(q,dq)*dq + G(q) = B*tau + E2*F_external +% + +%L=KE-PE; + +G=jacobian(PE,q).'; +G=simple(G); +D=simple(jacobian(KE,dq).'); +D=simple(jacobian(D,dq)); + +syms C real +n=max(size(q)); +for k=1:n + for j=1:n + C(k,j)=0*g; + for i=1:n + C(k,j)=C(k,j)+1/2*(diff(D(k,j),q(i)) + ... + diff(D(k,i),q(j)) - ... + diff(D(i,j),q(k)))*dq(i); + end + end +end +C=simple(C); +% +% Compute the matrix for the input torques and then the contact +% points. +% +Phi_0=[q31-q1;q32-q1;q41-q31;q42-q32;q1]; +B=jacobian(Phi_0,q); +B=B.'*[eye(4,4);zeros(1,4)]; +% +% +% F_ext = [F_T^1;F_N^1;F_T^2;F_N^2]; +% +Phi_1=[pFoot2]; +E=jacobian(Phi_1,q); +E=E.'; + +save mat_files/work_symb_model_abs_red + + +N=max(size(q)); + +%% First, output model to a file called +%%%% dynamics+.m +%% +% + +%% write file header +% +fcn_name = 'dyn_mod_abs_red'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function [D,C,G,B]=%s(x)\n',fcn_name); +fprintf(fid,['%% %s Grizzle''s model of kneed biped walker' ... + ' model.\n'],upper(fcn_name)); + +fprintf(fid,'%% [D,C,G,B] = %s',upper(fcn_name)); +fprintf(fid,'(X) is the kneed\n'); +fprintf(fid,'%% biped walking model. (x is of dimension %s)\n',... + num2str(2*N)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +%% Read in constants +% +fprintf(fid,['[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=' ... + ' modelParameters;\n\n']); + +%% Reassign configuration parameters +% +fprintf(fid,'q31=x(1); q32=x(2); q41=x(3); q42=x(4); q1=x(5);\n'); +fprintf(fid,['dq31=x(6); dq32=x(7); dq41=x(8); dq42=x(9); dq1=' ... + ' x(10);\n\n']); + +%% Model output +% +fprintf(fid,'%% D matrix\n'); +fprintf(fid,'D=zeros(%s);\n',num2str(N)); +for k=1:N, + for j=1:N, + if D(k,j)~=0 + ttt=char(D(k,j)); + fprintf(fid,'D(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end + +fprintf(fid,'\n%% C matrix\n'); +fprintf(fid,'C=zeros(%s);\n',num2str(N)); +for k=1:N, + for j=1:N, + if C(k,j)~=0 + ttt=char(C(k,j)); + fprintf(fid,'C(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end + +fprintf(fid,'\n%% G matrix\n'); +fprintf(fid,'G=zeros(%s,1);\n',num2str(N)); +for k=1:N, + if G(k)~=0 + ttt=char(G(k)); + fprintf(fid,'G(%s)=%s;\n',num2str(k),ttt); + end +end + +fprintf(fid,'\n%% B matrix\n'); +[N,M]=size(B); +fprintf(fid,'B=zeros(%s,%s);\n',num2str(N),num2str(M)); +for k=1:N, + for j=1:M, + if B(k,j)~=0 + ttt=char(B(k,j)); + fprintf(fid,'B(%s,%s)=%s;\n',num2str(k),num2str(j),ttt); + end + end +end +fclose(fid); + + +%% write file header +% +fcn_name = 'swing_foot_height'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function h = %s(x)\n',fcn_name); +fprintf(fid,'%% %s The swing foot height.\n',upper(fcn_name)); +fprintf(fid,'%% H = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); + +fprintf(fid,'[n,m] = size(x);\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +ttt = char(pFoot2(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' h = %s;\n',ttt); +fprintf(fid,'else\n'); +ttt = char(pFoot2(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' h = %s;\n',ttt); +fprintf(fid,'end\n'); +fclose(fid); + + +%% write file header +% +fcn_name = 'head_height'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function h = %s(x)\n',fcn_name); +fprintf(fid,'%% %s Head height.\n',upper(fcn_name)); +fprintf(fid,'%% H = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); + +fprintf(fid,'[n,m] = size(x);\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +ttt = char(pHead(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' h = %s;\n',ttt); +fprintf(fid,'else\n'); +ttt = char(pHead(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' h = %s;\n',ttt); +fprintf(fid,'end\n'); +fclose(fid); + + +%% write file header +% +fcn_name = 'limb_position'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function out = %s(x,pH_horiz)\n',fcn_name); +fprintf(fid,'%% %s Head height.\n',upper(fcn_name)); +fprintf(fid,'%% out = %s(X,PH_HORIZ)\n',upper(fcn_name)); +fprintf(fid,['%% The members are pH, pT, pHead, pFem1, pFem2, pG1,' ... + ' pG2, pTib1,\n%% pTib2, pFoot1, and pFoot2.\n\n']); + +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,['[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4]=' ... + ' modelParameters;\n\n']); + +pH_tmp = [0; pH(2)]; + +fprintf(fid,'[n,m] = size(x);\n'); +fprintf(fid,'if n == 10, k = m; else, k = n; end\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +fprintf(fid,' out.pH1 = pH_horiz*ones(k,1);\n'); +ttt = char(pH(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pH2 = %s;\n',ttt); +pT_tmp = pT - pH; +ttt = char(pT_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pT1 = out.pH1 + %s;\n',ttt); +ttt = char(pT_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pT2 = out.pH2 + %s;\n',ttt); +pHead_tmp = pHead - pH; +ttt = char(pHead_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pHead1 = out.pH1 + %s;\n',ttt); +ttt = char(pHead_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pHead2 = out.pH2 + %s;\n',ttt); +pFem1_tmp = pFem1 - pH; +ttt = char(pFem1_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFem11 = out.pH1 + %s;\n',ttt); +ttt = char(pFem1_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFem12 = out.pH2 + %s;\n',ttt); +pFem2_tmp = pFem2 - pH; +ttt = char(pFem2_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFem21 = out.pH1 + %s;\n',ttt); +ttt = char(pFem2_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFem22 = out.pH2 + %s;\n',ttt); +pG1_tmp = pG1 - pH; +ttt = char(pG1_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pG11 = out.pH1 + %s;\n',ttt); +ttt = char(pG1_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pG12 = out.pH2 + %s;\n',ttt); +pG2_tmp = pG2 - pH; +ttt = char(pG2_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pG21 = out.pH1 + %s;\n',ttt); +ttt = char(pG2_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pG22 = out.pH2 + %s;\n',ttt); +pTib1_tmp = pTib1 - pH; +ttt = char(pTib1_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pTib11 = out.pH1 + %s;\n',ttt); +ttt = char(pTib1_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pTib12 = out.pH2 + %s;\n',ttt); +pTib2_tmp = pTib2 - pH; +ttt = char(pTib2_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pTib21 = out.pH1 + %s;\n',ttt); +ttt = char(pTib2_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pTib22 = out.pH2 + %s;\n',ttt); +pFoot1_tmp = pFoot1 - pH; +ttt = char(pFoot1_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFoot11 = out.pH1 + %s;\n',ttt); +ttt = char(pFoot1_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFoot12 = out.pH2 + %s;\n',ttt); +pFoot2_tmp = pFoot2 - pH; +ttt = char(pFoot2_tmp(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFoot21 = out.pH1 + %s;\n',ttt); +ttt = char(pFoot2_tmp(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' out.pFoot22 = out.pH2 + %s;\n',ttt); +fprintf(fid,'else\n'); +fprintf(fid,' out.pH1 = pH_horiz;\n'); +ttt = char(pH(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pH2 = %s;\n',ttt); +pT_tmp = pT - pH; +ttt = char(pT_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pT1 = out.pH1 + %s;\n',ttt); +ttt = char(pT_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pT2 = out.pH2 + %s;\n',ttt); +pHead_tmp = pHead - pH; +ttt = char(pHead_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pHead1 = out.pH1 + %s;\n',ttt); +ttt = char(pHead_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pHead2 = out.pH2 + %s;\n',ttt); +pFem1_tmp = pFem1 - pH; +ttt = char(pFem1_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFem11 = out.pH1 + %s;\n',ttt); +ttt = char(pFem1_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFem12 = out.pH2 + %s;\n',ttt); +pFem2_tmp = pFem2 - pH; +ttt = char(pFem2_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFem21 = out.pH1 + %s;\n',ttt); +ttt = char(pFem2_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFem22 = out.pH2 + %s;\n',ttt); +pG1_tmp = pG1 - pH; +ttt = char(pG1_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pG11 = out.pH1 + %s;\n',ttt); +ttt = char(pG1_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pG12 = out.pH2 + %s;\n',ttt); +pG2_tmp = pG2 - pH; +ttt = char(pG2_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pG21 = out.pH1 + %s;\n',ttt); +ttt = char(pG2_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pG22 = out.pH2 + %s;\n',ttt); +pTib1_tmp = pTib1 - pH; +ttt = char(pTib1_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pTib11 = out.pH1 + %s;\n',ttt); +ttt = char(pTib1_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pTib12 = out.pH2 + %s;\n',ttt); +pTib2_tmp = pTib2 - pH; +ttt = char(pTib2_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pTib21 = out.pH1 + %s;\n',ttt); +ttt = char(pTib2_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pTib22 = out.pH2 + %s;\n',ttt); +pFoot1_tmp = pFoot1 - pH; +ttt = char(pFoot1_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFoot11 = out.pH1 + %s;\n',ttt); +ttt = char(pFoot1_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFoot12 = out.pH2 + %s;\n',ttt); +pFoot2_tmp = pFoot2 - pH; +ttt = char(pFoot2_tmp(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFoot21 = out.pH1 + %s;\n',ttt); +ttt = char(pFoot2_tmp(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' out.pFoot22 = out.pH2 + %s;\n',ttt); +fprintf(fid,'end\n'); +fclose(fid); + + +%% write file header +% +fcn_name = 'swing_foot_velocity'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function [vh,vv] = %s(x)\n',fcn_name); +fprintf(fid,'%% %s The swing foot vertical velocity.\n', ... + upper(fcn_name)); +fprintf(fid,'%% [VH,VV] = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); + +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); + +fprintf(fid,'[n,m] = size(x);\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +ttt = char(vFoot2(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' vh = %s;\n',ttt); +ttt = char(vFoot2(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' vv = %s;\n',ttt); +fprintf(fid,'else\n'); +ttt = char(vFoot2(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' vh = %s;\n',ttt); +ttt = char(vFoot2(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' vv = %s;\n',ttt); +fprintf(fid,'end'); +fclose(fid); + + +%% write file header +% +fcn_name = 'step_length'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function h = %s(x)\n',fcn_name); +fprintf(fid,'%% %s The step length.\n',upper(fcn_name)); +fprintf(fid,'%% H = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); + +%% Reassign configuration parameters +% +fprintf(fid,'q31=x(1); q32=x(2); q41=x(3); q42=x(4); q1=x(5);\n'); + +fprintf(fid,'h = %s;',fixlength(char(pFoot2(1)-pFoot1(1)),'+',70)); +fclose(fid); + + +%% write file header +% +fcn_name = 'hip_pos'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function [pHh,pHv] = %s(x)\n',fcn_name); +fprintf(fid,'%% %s The hip position.\n',upper(fcn_name)); +fprintf(fid,'%% [pHv,pHh] = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); + +fprintf(fid,'[n,m] = size(x);\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +ttt = char(pH(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' pHh = %s;\n',ttt); +ttt = char(pH(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' pHv = %s;\n',ttt); +fprintf(fid,'else\n'); +ttt = char(pH(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' pHh = %s;\n',ttt); +ttt = char(pH(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' pHv = %s;\n',ttt); +fprintf(fid,'end\n'); +fclose(fid); + + +%% write file header +% +fcn_name = 'hip_vel'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function [vHh,vHv] = %s(x)\n',fcn_name); +fprintf(fid,'%% %s The hip velocity.\n',upper(fcn_name)); +fprintf(fid,'%% [vHh,vHv] = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); +fprintf(fid,'[g,L1,L3,L4] = modelParameters;\n\n'); +ttt = char(vH(1)); +ttt = rep_str(ttt,1); +fprintf(fid,'vHh = %s;\n\n',fixlength(ttt,'+',70)); +ttt = char(vH(2)); +ttt = rep_str(ttt,1); +fprintf(fid,'vHv = %s;\n',fixlength(ttt,'+',70)); +fclose(fid); + + +%% write file header +% +fcn_name = 'total_energy_red'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function E = %s(x)\n',fcn_name); +fprintf(fid,'%% %s Total energy of robot, E = KE + PE.\n', ... + upper(fcn_name)); +fprintf(fid,'%% E = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); +fprintf(fid,['[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=' ... + ' modelParameters;\n\n']); + +fprintf(fid,'[n,m] = size(x);\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +ttt = char(KE+PE); +ttt = rep_str(ttt,1); +fprintf(fid,' E = %s;\n',fixlength(ttt,'*+-',70)); +fprintf(fid,'else\n'); +ttt = char(KE+PE); +ttt = rep_str(ttt,2); +fprintf(fid,' E = %s;\n',fixlength(ttt,'*+-',70)); +fprintf(fid,'end\n'); +fclose(fid); + + +%% write file header +% +fcn_name = 'center_of_mass'; +disp(['[creating ',upper(fcn_name),'.m]']); +fid = fopen([fcn_name,'.m'],'w'); +fprintf(fid,'function [cmh,cmv] = %s(x)\n',fcn_name); +fprintf(fid,'%% %s Robot center of mass.\n', ... + upper(fcn_name)); +fprintf(fid,'%% [CMH,CMV] = %s(X)\n\n',upper(fcn_name)); +fprintf(fid,'%%Eric Westervelt\n'); +fprintf(fid,'%%%s\n\n',datestr(now)); +fprintf(fid,['[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]=' ... + ' modelParameters;\n\n']); + +fprintf(fid,'[n,m] = size(x);\n\n'); +fprintf(fid,'if n ~= 1 & m ~= 1\n'); +ttt = char(cm(1)); +ttt = rep_str(ttt,1); +fprintf(fid,' cmh = %s;\n',fixlength(ttt,'*+-',70)); +ttt = char(cm(2)); +ttt = rep_str(ttt,1); +fprintf(fid,' cmv = %s;\n',fixlength(ttt,'*+-',70)); +fprintf(fid,'else\n'); +ttt = char(cm(1)); +ttt = rep_str(ttt,2); +fprintf(fid,' cmh = %s;\n',fixlength(ttt,'*+-',70)); +ttt = char(cm(2)); +ttt = rep_str(ttt,2); +fprintf(fid,' cmv = %s;\n',fixlength(ttt,'*+-',70)); +fprintf(fid,'end\n'); +fclose(fid); + + +%------------------------------------------------------ + +function ttt_out = rep_str(ttt,type) + +if type ==1 + ttt = strrep(ttt,'dq31','x(:,6)'); + ttt = strrep(ttt,'dq32','x(:,7)'); + ttt = strrep(ttt,'dq41','x(:,8)'); + ttt = strrep(ttt,'dq42','x(:,9)'); + ttt = strrep(ttt,'dq1','x(:,10)'); + ttt = strrep(ttt,'q31','x(:,1)'); + ttt = strrep(ttt,'q32','x(:,2)'); + ttt = strrep(ttt,'q41','x(:,3)'); + ttt = strrep(ttt,'q42','x(:,4)'); + ttt = strrep(ttt,'q1','x(:,5)'); + ttt = strrep(ttt,'*','.*'); + ttt = strrep(ttt,'^','.^'); +else + ttt = strrep(ttt,'dq31','x(6)'); + ttt = strrep(ttt,'dq32','x(7)'); + ttt = strrep(ttt,'dq41','x(8)'); + ttt = strrep(ttt,'dq42','x(9)'); + ttt = strrep(ttt,'dq1','x(10)'); + ttt = strrep(ttt,'q31','x(1)'); + ttt = strrep(ttt,'q32','x(2)'); + ttt = strrep(ttt,'q41','x(3)'); + ttt = strrep(ttt,'q42','x(4)'); + ttt = strrep(ttt,'q1','x(5)'); +end + +ttt_out = ttt; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/theta_obj.m b/demos/rabbit/anim/rabbit_model/theta_obj.m new file mode 100644 index 0000000..51cc4aa --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/theta_obj.m @@ -0,0 +1,16 @@ +function f = theta_obj(theta1) +%THETA_OBJ Function to find theta. +% +% F = theta_obj(THETA1) + +%Eric Westervelt +%01-May-2001 05:42:40 + +a = controlParameters; + +aN=a(7); bN=a(14); cN=a(21); dN=a(28); + +[g,L1,L3,L4] = modelParameters; + +f = -L3.*cos(-1./2.*cN+theta1)-L4.*cos(1./2.*cN+theta1)+L3.*cos(bN- ... + 1./2.*cN+theta1)+L4.*cos(bN-1./2.*cN+dN+theta1); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/tilefigs.m b/demos/rabbit/anim/rabbit_model/tilefigs.m new file mode 100644 index 0000000..1c13432 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/tilefigs.m @@ -0,0 +1,73 @@ +function tilefigs() +% tile figure windows + + +%Charles Plum Nichols Research Corp. +% 70 Westview Street +%Tel: (781) 862-9400 Kilnbrook IV +%Fax: (781) 862-9485 Lexington, MA 02173 + +% Deepak Aswani modified 2 Jan 2002 + + +% constants +left_window_margin = 150; +side_width = 10; +top_space = 70; +bottom_space = 5; +start_height = 30; + +maxpos = get (0,'screensize'); % determine terminal size in pixels +hands = get (0,'Children'); % locate all open figure handles +hands = sort(hands); % sort figure handles +numfigs = size(hands,1); % number of open figures +maxfigs = fix(maxpos(4)/20); + +if (numfigs>maxfigs) % figure limit check + disp([' More than ' num2str(maxfigs) ' requested ']) + return +end + +stop = 0; +for col = 1:10 + for row = 1:col + xdim = floor((maxpos(3) - left_window_margin)/col) - side_width; + ydim = floor((maxpos(4) - start_height)/row) - top_space - bottom_space; + if ((row*col) >= numfigs) + stop = 1; + break; + end; + end; + if stop + break + end; +end; + +% tile figures by postiion +% Location (1,1) is at bottom left corner +%pnum=0; +%for iy = 1:numfigs +% figure(hands(iy)) +% p = get(hands(iy),'Position'); % get figure position +% +% ypos = maxpos(4) - (iy-1)*20 -p(4) -45 ; % figure location (row) +% ypos = maxpos(4) - (iy-1)*20 -p(4) -45 ; % figure location (row) +% xpos = fix((iy-1)*5 + 15); % figure location (column) + +% set(hands(iy),'Position',[ xpos ypos p(3) p(4) ]); % move figure +%end + +fnum=0; +for icol = 1:col + for irow = 1:row + ypos = (irow - 1) * (ydim + top_space + bottom_space) + start_height + bottom_space; % figure location (row) + xpos = (icol - 1) * (xdim + side_width) + floor(side_width/2) + left_window_margin; % figure location (column) + fnum = fnum + 1; + if fnum <= numfigs + set(hands(fnum),'Position', [xpos ypos xdim ydim]); + end; + end; +end; + + +return; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/total_energy_red.m b/demos/rabbit/anim/rabbit_model/total_energy_red.m new file mode 100644 index 0000000..8e37aa9 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/total_energy_red.m @@ -0,0 +1,47 @@ +function E = total_energy_red(x) +% TOTAL_ENERGY_RED Total energy of robot, E = KE + PE. +% E = TOTAL_ENERGY_RED(X) + +%Eric Westervelt +%24-Mar-2001 10:46:13 + +[g,L1,L3,L4,M1,M3,M4,MY1,MZ1,MZ3,MZ4,XX1,XX3,XX4]= modelParameters; + +[n,m] = size(x); + +if n ~= 1 & m ~= 1 + E = -x(:,10).*MY1.*L4.*x(:,8).*sin(x(:,5)-x(:,3))-x(:,10).*MY1.*L3.* ... +x(:,6).*sin(x(:,5)-x(:,1))-MZ3.*x(:,6).*L4.*x(:,8).*cos(-x(:,1)+ ... +x(:,3))-x(:,10).*MZ1.*L3.*x(:,6).*cos(x(:,5)-x(:,1))-x(:,10).*MZ1.* ... +L4.*x(:,8).*cos(x(:,5)-x(:,3))-MZ3.*x(:,7).*L3.*x(:,6).*cos(x(:,2)- ... +x(:,1))-MZ3.*x(:,7).*L4.*x(:,8).*cos(-x(:,2)+x(:,3))-MZ4.*x(:,9).*L4.* ... +x(:,8).*cos(-x(:,4)+x(:,3))+MZ4.*x(:,9).*L3.*x(:,7).*cos(-x(:,4)+ ... +x(:,2))-MZ4.*x(:,9).*L3.*x(:,6).*cos(-x(:,4)+x(:,1))+M1.*L3.*x(:,6).* ... +L4.*x(:,8).*cos(-x(:,1)+x(:,3))-M4.*L3.^2.*x(:,6).*x(:,7).*cos(x(:,2)- ... +x(:,1))+2.*M3.*L3.*x(:,6).*L4.*x(:,8).*cos(-x(:,1)+x(:,3))-M4.*L3.* ... +x(:,7).*L4.*x(:,8).*cos(-x(:,2)+x(:,3))+M4.*L3.*x(:,6).*L4.*x(:,8).* ... +cos(-x(:,1)+x(:,3))+1/2.*M4.*L3.^2.*x(:,6).^2+M3.*L4.^2.*x(:,8).^2- ... +MZ3.*x(:,6).^2.*L3+M3.*L3.^2.*x(:,6).^2-MZ4.*x(:,8).^2.*L4+M4.*L4.^2.* ... +x(:,8).^2+1/2.*M1.*L4.^2.*x(:,8).^2+1/2.*M1.*L3.^2.*x(:,6).^2+1/2.* ... +M4.*L3.^2.*x(:,7).^2+1/2.*XX3.*x(:,6).^2+1/2.*XX4.*x(:,9).^2+1/2.* ... +XX4.*x(:,8).^2+1/2.*XX1.*x(:,10).^2+1/2.*XX3.*x(:,7).^2+g.*(-L3.* ... +cos(x(:,1)).*M1-L4.*cos(x(:,3)).*M1+cos(x(:,5)).*MZ1+sin(x(:,5)).*MY1- ... +2.*L3.*cos(x(:,1)).*M3-2.*L4.*cos(x(:,3)).*M3+MZ3.*cos(x(:,1))+MZ3.* ... +cos(x(:,2))-2.*L4.*cos(x(:,3)).*M4+MZ4.*cos(x(:,3))-L3.*cos(x(:,1)).* ... +M4+L3.*cos(x(:,2)).*M4+MZ4.*cos(x(:,4))); +else + E = -x(10)*MY1*L4*x(8)*sin(x(5)-x(3))-x(10)*MY1*L3*x(6)*sin(x(5)-x(1))- ... +MZ3*x(6)*L4*x(8)*cos(-x(1)+x(3))-x(10)*MZ1*L3*x(6)*cos(x(5)-x(1))- ... +x(10)*MZ1*L4*x(8)*cos(x(5)-x(3))-MZ3*x(7)*L3*x(6)*cos(x(2)-x(1))-MZ3* ... +x(7)*L4*x(8)*cos(-x(2)+x(3))-MZ4*x(9)*L4*x(8)*cos(-x(4)+x(3))+MZ4* ... +x(9)*L3*x(7)*cos(-x(4)+x(2))-MZ4*x(9)*L3*x(6)*cos(-x(4)+x(1))+M1*L3* ... +x(6)*L4*x(8)*cos(-x(1)+x(3))-M4*L3^2*x(6)*x(7)*cos(x(2)-x(1))+2*M3*L3* ... +x(6)*L4*x(8)*cos(-x(1)+x(3))-M4*L3*x(7)*L4*x(8)*cos(-x(2)+x(3))+M4*L3* ... +x(6)*L4*x(8)*cos(-x(1)+x(3))+1/2*M4*L3^2*x(6)^2+M3*L4^2*x(8)^2-MZ3* ... +x(6)^2*L3+M3*L3^2*x(6)^2-MZ4*x(8)^2*L4+M4*L4^2*x(8)^2+1/2*M1*L4^2* ... +x(8)^2+1/2*M1*L3^2*x(6)^2+1/2*M4*L3^2*x(7)^2+1/2*XX3*x(6)^2+1/2*XX4* ... +x(9)^2+1/2*XX4*x(8)^2+1/2*XX1*x(10)^2+1/2*XX3*x(7)^2+g*(-L3*cos(x(1))* ... +M1-L4*cos(x(3))*M1+cos(x(5))*MZ1+sin(x(5))*MY1-2*L3*cos(x(1))*M3-2*L4* ... +cos(x(3))*M3+MZ3*cos(x(1))+MZ3*cos(x(2))-2*L4*cos(x(3))*M4+MZ4* ... +cos(x(3))-L3*cos(x(1))*M4+L3*cos(x(2))*M4+MZ4*cos(x(4))); +end diff --git a/demos/rabbit/anim/rabbit_model/vectfield.m b/demos/rabbit/anim/rabbit_model/vectfield.m new file mode 100644 index 0000000..4fed74b --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/vectfield.m @@ -0,0 +1,33 @@ +function vectfield(func,y1val,y2val,scl,opt) +%VECTFIELD vector field for system of 2 first order ODEs +% VECTFIELD(FUNC,Y1VAL,Y2VAL) plots the vector field for the system of +% two first order ODEs given by func, using the grid of y1val and y2 +% values given by the vectors y1val and y2val. func is either a the name +% of an inline function of two variables, or a string with the name of an +% m-file. By default, t=0 is used in func. A t value can be specified as +% an additional argument: vectfield(func,y1val,y2val,t) + + +n1 = length(y1val); +n2 = length(y2val); +yp1 = zeros(n2,n1); +yp2 = zeros(n2,n1); +for i = 1:n1 + for j = 1:n2 + if nargin < 5 + ypv = feval(func,[y1val(i);y2val(j)]); + yp1(j,i) = ypv(1); + yp2(j,i) = ypv(2); + else + ypv = feval(func,[y1val(i);y2val(j)],opt); + yp1(j,i) = ypv(1); + yp2(j,i) = ypv(2); + end + end +end +if nargin < 4 + quiver_erw(y1val,y2val,yp1,yp2,1); +else + quiver_erw(y1val,y2val,yp1,yp2,scl); +end +axis tight; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/zd.m b/demos/rabbit/anim/rabbit_model/zd.m new file mode 100644 index 0000000..6431818 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd.m @@ -0,0 +1,100 @@ +function dz = zd(z) +%ZD + +%Eric Westervelt +%01-May-2001 07:44:49 + +[n,m] = size(z); +if n == 1 | m == 1 + z1 = z(1); z2 = z(2); +else + z1 = z(:,1); z2 = z(:,2); +end + +[a,b,m] = controlParameters; + +a0=a(1); a1=a(2); a2=a(3); a3=a(4); a4=a(5); a5=a(6); a6=a(7); +b0=a(8); b1=a(9); b2=a(10); b3=a(11); b4=a(12); b5=a(13); b6=a(14); +c0=a(15); c1=a(16); c2=a(17); c3=a(18); c4=a(19); c5=a(20); c6=a(21); +d0=a(22); d1=a(23); d2=a(24); d3=a(25); d4=a(26); d5=a(27); d6=a(28); + +P1 = a0.*(1-(z1-b)./m).^6+6.*a1.*(z1-b)./m.*(1-(z1-b)./m).^5+15.*a2.* ... + (z1-b).^2./m.^2.*(1-(z1-b)./m).^4+20.*a3.*(z1-b).^3./m.^3.*(1- ... + (z1-b)./m).^3+15.*a4.*(z1-b).^4./m.^4.*(1-(z1-b)./m).^2+6.*a5.* ... + (z1-b).^5./m.^5.*(1-(z1-b)./m)+a6.*(z1-b).^6./m.^6; + +P2 = b0.*(1-(z1-b)./m).^6+6.*b1.*(z1-b)./m.*(1-(z1-b)./m).^5+15.*b2.* ... + (z1-b).^2./m.^2.*(1-(z1-b)./m).^4+20.*b3.*(z1-b).^3./m.^3.*(1- ... + (z1-b)./m).^3+15.*b4.*(z1-b).^4./m.^4.*(1-(z1-b)./m).^2+6.*b5.* ... + (z1-b).^5./m.^5.*(1-(z1-b)./m)+b6.*(z1-b).^6./m.^6; + +P3 = c0.*(1-(z1-b)./m).^6+6.*c1.*(z1-b)./m.*(1-(z1-b)./m).^5+15.*c2.* ... + (z1-b).^2./m.^2.*(1-(z1-b)./m).^4+20.*c3.*(z1-b).^3./m.^3.*(1- ... + (z1-b)./m).^3+15.*c4.*(z1-b).^4./m.^4.*(1-(z1-b)./m).^2+6.*c5.* ... + (z1-b).^5./m.^5.*(1-(z1-b)./m)+c6.*(z1-b).^6./m.^6; + +P4 = d0.*(1-(z1-b)./m).^6+6.*d1.*(z1-b)./m.*(1-(z1-b)./m).^5+15.*d2.* ... + (z1-b).^2./m.^2.*(1-(z1-b)./m).^4+20.*d3.*(z1-b).^3./m.^3.*(1- ... + (z1-b)./m).^3+15.*d4.*(z1-b).^4./m.^4.*(1-(z1-b)./m).^2+6.*d5.* ... + (z1-b).^5./m.^5.*(1-(z1-b)./m)+d6.*(z1-b).^6./m.^6; + +jac_P1 = -6.*a0.*(1-(z1-b)./m).^5./m+6.*a1./m.*(1-(z1-b)./m).^5-30.*a1.* ... + (z1-b)./m.^2.*(1-(z1-b)./m).^4+30.*a2.*(z1-b)./m.^2.*(1-(z1- ... + b)./m).^4-60.*a2.*(z1-b).^2./m.^3.*(1-(z1-b)./m).^3+60.*a3.*(z1- ... + b).^2./m.^3.*(1-(z1-b)./m).^3-60.*a3.*(z1-b).^3./m.^4.*(1-(z1- ... + b)./m).^2+60.*a4.*(z1-b).^3./m.^4.*(1-(z1-b)./m).^2-30.*a4.*(z1- ... + b).^4./m.^5.*(1-(z1-b)./m)+30.*a5.*(z1-b).^4./m.^5.*(1-(z1- ... + b)./m)-6.*a5.*(z1-b).^5./m.^6+6.*a6.*(z1-b).^5./m.^6; + +jac_P2 = -6.*b0.*(1-(z1-b)./m).^5./m+6.*b1./m.*(1-(z1-b)./m).^5-30.*b1.* ... + (z1-b)./m.^2.*(1-(z1-b)./m).^4+30.*b2.*(z1-b)./m.^2.*(1-(z1- ... + b)./m).^4-60.*b2.*(z1-b).^2./m.^3.*(1-(z1-b)./m).^3+60.*b3.*(z1- ... + b).^2./m.^3.*(1-(z1-b)./m).^3-60.*b3.*(z1-b).^3./m.^4.*(1-(z1- ... + b)./m).^2+60.*b4.*(z1-b).^3./m.^4.*(1-(z1-b)./m).^2-30.*b4.*(z1- ... + b).^4./m.^5.*(1-(z1-b)./m)+30.*b5.*(z1-b).^4./m.^5.*(1-(z1- ... + b)./m)-6.*b5.*(z1-b).^5./m.^6+6.*b6.*(z1-b).^5./m.^6; + +jac_P3 = -6.*c0.*(1-(z1-b)./m).^5./m+6.*c1./m.*(1-(z1-b)./m).^5-30.*c1.* ... + (z1-b)./m.^2.*(1-(z1-b)./m).^4+30.*c2.*(z1-b)./m.^2.*(1-(z1- ... + b)./m).^4-60.*c2.*(z1-b).^2./m.^3.*(1-(z1-b)./m).^3+60.*c3.*(z1- ... + b).^2./m.^3.*(1-(z1-b)./m).^3-60.*c3.*(z1-b).^3./m.^4.*(1-(z1- ... + b)./m).^2+60.*c4.*(z1-b).^3./m.^4.*(1-(z1-b)./m).^2-30.*c4.*(z1- ... + b).^4./m.^5.*(1-(z1-b)./m)+30.*c5.*(z1-b).^4./m.^5.*(1-(z1- ... + b)./m)-6.*c5.*(z1-b).^5./m.^6+6.*c6.*(z1-b).^5./m.^6; + +jac_P4 = -6.*d0.*(1-(z1-b)./m).^5./m+6.*d1./m.*(1-(z1-b)./m).^5-30.*d1.* ... + (z1-b)./m.^2.*(1-(z1-b)./m).^4+30.*d2.*(z1-b)./m.^2.*(1-(z1- ... + b)./m).^4-60.*d2.*(z1-b).^2./m.^3.*(1-(z1-b)./m).^3+60.*d3.*(z1- ... + b).^2./m.^3.*(1-(z1-b)./m).^3-60.*d3.*(z1-b).^3./m.^4.*(1-(z1- ... + b)./m).^2+60.*d4.*(z1-b).^3./m.^4.*(1-(z1-b)./m).^2-30.*d4.*(z1- ... + b).^4./m.^5.*(1-(z1-b)./m)+30.*d5.*(z1-b).^4./m.^5.*(1-(z1- ... + b)./m)-6.*d5.*(z1-b).^5./m.^6+6.*d6.*(z1-b).^5./m.^6; + +s(1) = -P1+1./2.*P3+z1; +s(2) = P1+1./2.*P3-z1; + +% dz1 +dz_1 = 500.*z2./(400.*jac_P3.*cos(s(2))+20.*jac_P3.*sin(s(2))-40.* ... + jac_P1.*sin(s(2))-800.*jac_P1.*cos(s(2))+20.*jac_P3.*sin(s(1))- ... + 800.*jac_P1.*cos(s(1))+15.*jac_P3+50.*jac_P4-800.*cos(s(1))+40.* ... + sin(s(1))-400.*jac_P3.*cos(s(1))+5444.*cos(P3)-956.*cos(-P2+P3)+ ... + 40.*jac_P1.*sin(s(1))+6142-956.*cos(P2)+478.*jac_P3.*cos(P2)- ... + 478.*jac_P2.*cos(P2)-800.*cos(s(2))-40.*sin(s(2))+1110.*jac_P1- ... + 478.*jac_P2.*cos(-P2+P3)+164.*cos(P4)-164.*cos(-P2-P4+P3)-164.* ... + cos(P2+P4)+431.*jac_P2+82.*jac_P4.*cos(P4)+164.*jac_P2.*cos(P4)- ... + 82.*jac_P3.*cos(P4)-82.*jac_P4.*cos(-P2-P4+P3)-82.*jac_P4.* ... + cos(P2+P4)+82.*jac_P3.*cos(P2+P4)-82.*jac_P2.*cos(P2+P4)-82.* ... + jac_P2.*cos(-P2-P4+P3)); + +% dz2 +dz_2 = 4697609773505525./35184372088832.*sin(1./2.*P3-z1)- ... + 1345255995021145./8796093022208.*sin(1./2.*P3+z1)+981./25.* ... + sin(P1)-981./500.*cos(P1)-3299717078230185./140737488355328.* ... + sin(-P2+1./2.*P3-z1)-40221./10000.*sin(-P2+1./2.*P3-z1-P4); + +[n,m] = size(z); +if n == 1 | m == 1 + dz = [dz_1; dz_2]; +else + dz = [dz_1, dz_2]; +end \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/zd_cc.m b/demos/rabbit/anim/rabbit_model/zd_cc.m new file mode 100644 index 0000000..003f839 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_cc.m @@ -0,0 +1,1539 @@ +function dz = zd_cc(z) +%ZD_CC + +%Eric Westervelt +%26-Feb-2001 20:35:19 + +z1 = z(1); z2 = z(2); + +[a,b,m] = controlParameters; + +a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9); a5=a(10); +b0=a(11); b1=a(12); b2=a(13); b3=a(14); b4=a(15); b5=a(16); +c0=a(17); c1=a(18); c2=a(19); c3=a(20); c4=a(21); c5=a(22); +d0=a(23); d1=a(24); d2=a(25); d3=a(26); d4=a(27); d5=a(28); + +Ph1 = -a0*(-72357/12788+6250/3197*z1)^6+37500/3197*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-585937500/10220809*a2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+4882812500000/32675926373*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-22888183593750000/104464936614481*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+57220458984375000000/333974402356495757*a5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-59604644775390625000000/1067716164333716935129*a0*(z1-17029/5000)^6; + +Ph2 = -b0*(-72357/12788+6250/3197*z1)^6+37500/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-585937500/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+4882812500000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-22888183593750000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+57220458984375000000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-59604644775390625000000/1067716164333716935129*b0*(z1-17029/5000)^6; + +Ph3 = -c0*(-72357/12788+6250/3197*z1)^6+37500/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-585937500/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+4882812500000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-22888183593750000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+57220458984375000000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-59604644775390625000000/1067716164333716935129*d0*(z1-17029/5000)^6; + +Ph4 = -d0*(-72357/12788+6250/3197*z1)^6+37500/3197*d1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-585937500/10220809*d2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+4882812500000/32675926373*d3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-22888183593750000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+57220458984375000000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-59604644775390625000000/1067716164333716935129*c0*(z1-17029/5000)^6; + +s(1) = c5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(2) = c4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(3) = c3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(4) = c2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(5) = c1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(6) = c0*(-72357/12788+6250/3197*z1)^6; +s(7) = d5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(8) = d4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(9) = d3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(10) = d2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(11) = d1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(12) = d0*(-72357/12788+6250/3197*z1)^6; +s(13) = b5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(14) = b4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(15) = b3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(16) = b2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(17) = b1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(18) = b0*(-72357/12788+6250/3197*z1)^6; +s(19) = cos(1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6+1/2*d0*(-72357/12788+6250/3197*z1)^6-18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5+292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6-2* ... +z1+1/2*c0*(-72357/12788+6250/3197*z1)^6-18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6); +s(20) = c5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(21) = c4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(22) = c4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(23) = c3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(24) = c3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(25) = c2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(26) = c2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(27) = c1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(28) = c1*(-72357/12788+6250/3197*z1)^5; +s(29) = c0*(-72357/12788+6250/3197*z1)^5; +s(30) = cos(1/2*c0*(-72357/12788+6250/3197*z1)^6-18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6-2* ... +z1+1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6-1/2*d0*(-72357/12788+6250/3197*z1)^6+18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5-292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6); +s(31) = a5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(32) = a4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(33) = a3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(34) = a2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(35) = a1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(36) = a0*(-72357/12788+6250/3197*z1)^6; +s(37) = a0*(-72357/12788+6250/3197*z1)^6-37500/3197*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+585937500/10220809*a2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-4882812500000/32675926373*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+22888183593750000/104464936614481*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-57220458984375000000/333974402356495757*a5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+59604644775390625000000/1067716164333716935129*a0* ... +(z1-17029/5000)^6-1/2*c0*(-72357/12788+6250/3197*z1)^6+18750/3197*c1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5-292968750/10220809*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373* ... +c3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6-z1; +s(38) = a5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(39) = a4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(40) = a4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(41) = a3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(42) = a3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(43) = a2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(44) = a2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(45) = a1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(46) = a1*(-72357/12788+6250/3197*z1)^5; +s(47) = a0*(-72357/12788+6250/3197*z1)^5; +s(48) = b5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(49) = b4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(50) = b4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(51) = b3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(52) = b3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(53) = b2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(54) = b2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(55) = b1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(56) = b1*(-72357/12788+6250/3197*z1)^5; +s(57) = b0*(-72357/12788+6250/3197*z1)^5; +s(58) = cos(1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6-1/2*d0*(-72357/12788+6250/3197*z1)^6+18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5-292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6-2* ... +z1-1/2*c0*(-72357/12788+6250/3197*z1)^6+18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6); +s(59) = d5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(60) = d4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(61) = d4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(62) = d3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(63) = d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(64) = d2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(65) = d2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(66) = d1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(67) = d1*(-72357/12788+6250/3197*z1)^5; +s(68) = d0*(-72357/12788+6250/3197*z1)^5; +s(69) = cos(1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6+1/2*d0*(-72357/12788+6250/3197*z1)^6-18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5+292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6-2* ... +z1-1/2*c0*(-72357/12788+6250/3197*z1)^6+18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6); +s(70) = a0*(-72357/12788+6250/3197*z1)^6-37500/3197*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+585937500/10220809*a2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-4882812500000/32675926373*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+22888183593750000/104464936614481*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-57220458984375000000/333974402356495757*a5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+59604644775390625000000/1067716164333716935129*a0* ... +(z1-17029/5000)^6+1/2*c0*(-72357/12788+6250/3197*z1)^6-18750/3197*c1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5+292968750/10220809*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373* ... +c3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6-z1; + +% alpha +alpha = 1000/(-635147094726562500000000/333974402356495757* ... +s(38)+30303955078125000000/104464936614481*s(62)+387890625000/10220809* ... +s(66)+83250000/3197*s(47)-94699859619140625000000/333974402356495757* ... +s(60)-42205810546875000000/104464936614481*s(23)-16162500/3197* ... +s(56)+6752929687500000/32675926373* ... +s(25)+39459228515625000000/104464936614481* ... +s(50)+6313476562500000/32675926373*s(53)+32519531250000000/32675926373* ... +s(43)-83250000/3197*s(46)-4848632812500000/32675926373*s(64)-478*s(30)* ... +(-37500/3197*s(68)+37500/3197*s(67)+1171875000/10220809* ... +s(66)-1171875000/10220809*s(65)-14648437500000/32675926373* ... +s(64)+14648437500000/32675926373*s(63)+91552734375000000/104464936614481* ... +s(62)-91552734375000000/104464936614481* ... +s(61)-286102294921875000000/333974402356495757* ... +s(60)+286102294921875000000/333974402356495757* ... +s(59)+357627868652343750000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5)+17287500/3197*s(29)+478*s(30)*(-37500/3197* ... +s(57)+37500/3197*s(56)+1171875000/10220809*s(55)-1171875000/10220809* ... +s(54)-14648437500000/32675926373*s(53)+14648437500000/32675926373* ... +s(52)+91552734375000000/104464936614481* ... +s(51)-91552734375000000/104464936614481* ... +s(50)-286102294921875000000/333974402356495757* ... +s(49)+286102294921875000000/333974402356495757* ... +s(48)+357627868652343750000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)-(-60000000/3197*s(47)+60000000/3197* ... +s(46)+1875000000000/10220809*s(45)-1875000000000/10220809* ... +s(44)-23437500000000000/32675926373*s(43)+23437500000000000/32675926373* ... +s(42)+146484375000000000000/104464936614481* ... +s(41)-146484375000000000000/104464936614481* ... +s(40)-457763671875000000000000/333974402356495757* ... +s(39)+457763671875000000000000/333974402356495757* ... +s(38)+572204589843750000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5-572204589843750000000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5)*sin(s(37))+(-3000000/3197*s(47)+3000000/3197* ... +s(46)+93750000000/10220809*s(45)-93750000000/10220809* ... +s(44)-1171875000000000/32675926373*s(43)+1171875000000000/32675926373* ... +s(42)+7324218750000000000/104464936614481* ... +s(41)-7324218750000000000/104464936614481* ... +s(40)-22888183593750000000000/333974402356495757* ... +s(39)+22888183593750000000000/333974402356495757* ... +s(38)+28610229492187500000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5-28610229492187500000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5)*cos(s(37))-(-3000000/3197*s(47)+3000000/3197* ... +s(46)+93750000000/10220809*s(45)-93750000000/10220809* ... +s(44)-1171875000000000/32675926373*s(43)+1171875000000000/32675926373* ... +s(42)+7324218750000000000/104464936614481* ... +s(41)-7324218750000000000/104464936614481* ... +s(40)-22888183593750000000000/333974402356495757* ... +s(39)+22888183593750000000000/333974402356495757* ... +s(38)+28610229492187500000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5-28610229492187500000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5)*cos(s(70))+(-60000000/3197*s(47)+60000000/3197* ... +s(46)+1875000000000/10220809*s(45)-1875000000000/10220809* ... +s(44)-23437500000000000/32675926373*s(43)+23437500000000000/32675926373* ... +s(42)+146484375000000000000/104464936614481* ... +s(41)-146484375000000000000/104464936614481* ... +s(40)-457763671875000000000000/333974402356495757* ... +s(39)+457763671875000000000000/333974402356495757* ... +s(38)+572204589843750000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5-572204589843750000000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5)*sin(s(70))+42205810546875000000/104464936614481* ... +s(22)+10560+203247070312500000000/104464936614481*s(40)+(-1500000/3197* ... +s(29)+1500000/3197*s(28)+46875000000/10220809*s(27)-46875000000/10220809* ... +s(26)-585937500000000/32675926373*s(25)+585937500000000/32675926373* ... +s(24)+3662109375000000000/104464936614481* ... +s(23)-3662109375000000000/104464936614481* ... +s(22)-11444091796875000000000/333974402356495757* ... +s(21)+11444091796875000000000/333974402356495757* ... +s(20)+14305114746093750000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-14305114746093750000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)*cos(s(70))+(-1500000/3197*s(29)+1500000/3197* ... +s(28)+46875000000/10220809*s(27)-46875000000/10220809* ... +s(26)-585937500000000/32675926373*s(25)+585937500000000/32675926373* ... +s(24)+3662109375000000000/104464936614481* ... +s(23)-3662109375000000000/104464936614481* ... +s(22)-11444091796875000000000/333974402356495757* ... +s(21)+11444091796875000000000/333974402356495757* ... +s(20)+14305114746093750000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-14305114746093750000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)*cos(s(37))-(-30000000/3197*s(29)+30000000/3197* ... +s(28)+937500000000/10220809*s(27)-937500000000/10220809* ... +s(26)-11718750000000000/32675926373*s(25)+11718750000000000/32675926373* ... +s(24)+73242187500000000000/104464936614481* ... +s(23)-73242187500000000000/104464936614481* ... +s(22)-228881835937500000000000/333974402356495757* ... +s(21)+228881835937500000000000/333974402356495757* ... +s(20)+286102294921875000000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-286102294921875000000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)*sin(s(70))+(-3075000/3197*s(29)+3075000/3197* ... +s(28)+96093750000/10220809*s(27)-96093750000/10220809* ... +s(26)-1201171875000000/32675926373*s(25)+1201171875000000/32675926373* ... +s(24)+7507324218750000000/104464936614481* ... +s(23)-7507324218750000000/104464936614481* ... +s(22)-23460388183593750000000/333974402356495757* ... +s(21)+23460388183593750000000/333974402356495757* ... +s(20)+29325485229492187500000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)*s(69)-478*s(30)*(-37500/3197*s(29)+37500/3197* ... +s(28)+1171875000/10220809*s(27)-1171875000/10220809* ... +s(26)-14648437500000/32675926373*s(25)+14648437500000/32675926373* ... +s(24)+91552734375000000/104464936614481* ... +s(23)-91552734375000000/104464936614481* ... +s(22)-286102294921875000000/333974402356495757* ... +s(21)+286102294921875000000/333974402356495757* ... +s(20)+357627868652343750000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)-(-30000000/3197*s(29)+30000000/3197* ... +s(28)+937500000000/10220809*s(27)-937500000000/10220809* ... +s(26)-11718750000000000/32675926373*s(25)+11718750000000000/32675926373* ... +s(24)+73242187500000000000/104464936614481* ... +s(23)-73242187500000000000/104464936614481* ... +s(22)-228881835937500000000000/333974402356495757* ... +s(21)+228881835937500000000000/333974402356495757* ... +s(20)+286102294921875000000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-286102294921875000000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)*sin(s(37))-478*s(58)*(-37500/3197*s(29)+37500/3197* ... +s(28)+1171875000/10220809*s(27)-1171875000/10220809* ... +s(26)-14648437500000/32675926373*s(25)+14648437500000/32675926373* ... +s(24)+91552734375000000/104464936614481* ... +s(23)-91552734375000000/104464936614481* ... +s(22)-286102294921875000000/333974402356495757* ... +s(21)+286102294921875000000/333974402356495757* ... +s(20)+357627868652343750000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)-80*cos(s(37))+1600* ... +sin(s(37))-154137611389160156250000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5+154137611389160156250000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5-10888*cos(s(6)-37500/3197*s(5)+585937500/10220809* ... +s(4)-4882812500000/32675926373*s(3)+22888183593750000/104464936614481* ... +s(2)-57220458984375000000/333974402356495757* ... +s(1)+59604644775390625000000/1067716164333716935129*d0* ... +(z1-17029/5000)^6)-6313476562500000/32675926373* ... +s(52)+123310089111328125000000/333974402356495757* ... +s(49)+635147094726562500000000/333974402356495757* ... +s(39)-30303955078125000000/104464936614481* ... +s(61)+118374824523925781250000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-118374824523925781250000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5-164866447448730468750000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5+164866447448730468750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5-(-3075000/3197*s(68)+3075000/3197* ... +s(67)+96093750000/10220809*s(66)-96093750000/10220809* ... +s(65)-1201171875000000/32675926373*s(64)+1201171875000000/32675926373* ... +s(63)+7507324218750000000/104464936614481* ... +s(62)-7507324218750000000/104464936614481* ... +s(61)-23460388183593750000000/333974402356495757* ... +s(60)+23460388183593750000000/333974402356495757* ... +s(59)+29325485229492187500000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5)*s(19)-(-3075000/3197*s(57)+3075000/3197* ... +s(56)+96093750000/10220809*s(55)-96093750000/10220809* ... +s(54)-1201171875000000/32675926373*s(53)+1201171875000000/32675926373* ... +s(52)+7507324218750000000/104464936614481* ... +s(51)-7507324218750000000/104464936614481* ... +s(50)-23460388183593750000000/333974402356495757* ... +s(49)+23460388183593750000000/333974402356495757* ... +s(48)+29325485229492187500000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)*s(19)-123310089111328125000000/333974402356495757* ... +s(48)+(-3075000/3197*s(29)+3075000/3197*s(28)+96093750000/10220809* ... +s(27)-96093750000/10220809*s(26)-1201171875000000/32675926373* ... +s(25)+1201171875000000/32675926373* ... +s(24)+7507324218750000000/104464936614481* ... +s(23)-7507324218750000000/104464936614481* ... +s(22)-23460388183593750000000/333974402356495757* ... +s(21)+23460388183593750000000/333974402356495757* ... +s(20)+29325485229492187500000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)*s(19)-505078125000/10220809* ... +s(55)+2601562500000/10220809*s(44)+540234375000/10220809* ... +s(26)+505078125000/10220809*s(54)-32519531250000000/32675926373* ... +s(42)-39459228515625000000/104464936614481* ... +s(51)-793933868408203125000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5+793933868408203125000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5+478*s(58)*(-37500/3197*s(68)+37500/3197* ... +s(67)+1171875000/10220809*s(66)-1171875000/10220809* ... +s(65)-14648437500000/32675926373*s(64)+14648437500000/32675926373* ... +s(63)+91552734375000000/104464936614481* ... +s(62)-91552734375000000/104464936614481* ... +s(61)-286102294921875000000/333974402356495757* ... +s(60)+286102294921875000000/333974402356495757* ... +s(59)+357627868652343750000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5)-540234375000/10220809* ... +s(27)+131893157958984375000000/333974402356495757*s(21)+12412500/3197* ... +s(67)-203247070312500000000/104464936614481*s(41)-17287500/3197*s(28)+328* ... +cos(s(12)-37500/3197*s(11)+585937500/10220809* ... +s(10)-4882812500000/32675926373*s(9)+22888183593750000/104464936614481* ... +s(8)-57220458984375000000/333974402356495757* ... +s(7)+59604644775390625000000/1067716164333716935129*c0* ... +(z1-17029/5000)^6)-131893157958984375000000/333974402356495757* ... +s(20)-12412500/3197*s(68)+(-6150000/3197*s(57)+6150000/3197* ... +s(56)+192187500000/10220809*s(55)-192187500000/10220809* ... +s(54)-2402343750000000/32675926373*s(53)+2402343750000000/32675926373* ... +s(52)+15014648437500000000/104464936614481* ... +s(51)-15014648437500000000/104464936614481* ... +s(50)-46920776367187500000000/333974402356495757* ... +s(49)+46920776367187500000000/333974402356495757* ... +s(48)+58650970458984375000000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-58650970458984375000000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)*cos(s(12)-37500/3197*s(11)+585937500/10220809* ... +s(10)-4882812500000/32675926373*s(9)+22888183593750000/104464936614481* ... +s(8)-57220458984375000000/333974402356495757* ... +s(7)+59604644775390625000000/1067716164333716935129*c0* ... +(z1-17029/5000)^6)-6752929687500000/32675926373*s(24)+16162500/3197* ... +s(57)+4848632812500000/32675926373* ... +s(63)+94699859619140625000000/333974402356495757* ... +s(59)-2601562500000/10220809*s(45)+80*cos(s(70))-1600* ... +sin(s(70))-387890625000/10220809*s(65)+(-3075000/3197*s(57)+3075000/3197* ... +s(56)+96093750000/10220809*s(55)-96093750000/10220809* ... +s(54)-1201171875000000/32675926373*s(53)+1201171875000000/32675926373* ... +s(52)+7507324218750000000/104464936614481* ... +s(51)-7507324218750000000/104464936614481* ... +s(50)-23460388183593750000000/333974402356495757* ... +s(49)+23460388183593750000000/333974402356495757* ... +s(48)+29325485229492187500000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)*s(69)-478*s(58)*(-37500/3197*s(57)+37500/3197* ... +s(56)+1171875000/10220809*s(55)-1171875000/10220809* ... +s(54)-14648437500000/32675926373*s(53)+14648437500000/32675926373* ... +s(52)+91552734375000000/104464936614481* ... +s(51)-91552734375000000/104464936614481* ... +s(50)-286102294921875000000/333974402356495757* ... +s(49)+286102294921875000000/333974402356495757* ... +s(48)+357627868652343750000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)+(-3075000/3197*s(68)+3075000/3197* ... +s(67)+96093750000/10220809*s(66)-96093750000/10220809* ... +s(65)-1201171875000000/32675926373*s(64)+1201171875000000/32675926373* ... +s(63)+7507324218750000000/104464936614481* ... +s(62)-7507324218750000000/104464936614481* ... +s(61)-23460388183593750000000/333974402356495757* ... +s(60)+23460388183593750000000/333974402356495757* ... +s(59)+29325485229492187500000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5)*s(69)); + +s(1) = b5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(2) = b4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(3) = b4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(4) = b3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(5) = b3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(6) = b2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(7) = b2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(8) = b1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(9) = b1*(-72357/12788+6250/3197*z1)^5; +s(10) = b0*(-72357/12788+6250/3197*z1)^5; +s(11) = -37500/3197*b0*(-72357/12788+6250/3197*z1)^5+37500/3197*b1* ... +(-72357/12788+6250/3197*z1)^5+1171875000/10220809*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1171875000/10220809*b2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-14648437500000/32675926373*b2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+14648437500000/32675926373* ... +b3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+91552734375000000/104464936614481*b3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-91552734375000000/104464936614481*b4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-286102294921875000000/333974402356495757*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+286102294921875000000/333974402356495757*b5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+357627868652343750000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5; +s(12) = d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(13) = d2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(14) = d1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(15) = d2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(16) = c5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(17) = c4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(18) = c4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(19) = c3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(20) = c3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(21) = c2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(22) = c1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(23) = c2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(24) = d0*(-72357/12788+6250/3197*z1)^5; +s(25) = d1*(-72357/12788+6250/3197*z1)^5; +s(26) = d5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(27) = d4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(28) = d4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(29) = d3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(30) = c1*(-72357/12788+6250/3197*z1)^5; +s(31) = c0*(-72357/12788+6250/3197*z1)^5; +s(32) = -2-178813934326171875000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5+18750/3197*c0*(-72357/12788+6250/3197*z1)^5-18750/3197* ... +c1*(-72357/12788+6250/3197*z1)^5+18750/3197*b0*(-72357/12788+6250/3197* ... +z1)^5-18750/3197*b1*(-72357/12788+6250/3197*z1)^5-585937500/10220809*b1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+585937500/10220809*b2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*b2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +b3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+178813934326171875000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5+45776367187500000/104464936614481*d3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-45776367187500000/104464936614481*d4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-143051147460937500000/333974402356495757*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+143051147460937500000/333974402356495757*d5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)+18750/3197*d1* ... +(-72357/12788+6250/3197*z1)^5-18750/3197*d0*(-72357/12788+6250/3197* ... +z1)^5-178813934326171875000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5-178813934326171875000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5+585937500/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-585937500/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-45776367187500000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+45776367187500000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+143051147460937500000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-143051147460937500000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+45776367187500000/104464936614481*b4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+143051147460937500000/333974402356495757*b4* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)-143051147460937500000/333974402356495757*b5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-45776367187500000/104464936614481*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2-585937500/10220809*d2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+585937500/10220809*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4-7324218750000/32675926373*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+7324218750000/32675926373* ... +d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(33) = d5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(34) = d4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(35) = d3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(36) = d2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(37) = d1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(38) = d0*(-72357/12788+6250/3197*z1)^6; +s(39) = b5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(40) = b4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(41) = b3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(42) = b2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(43) = b1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(44) = b0*(-72357/12788+6250/3197*z1)^6; +s(45) = c5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(46) = c4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(47) = c3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(48) = c2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(49) = c1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(50) = c0*(-72357/12788+6250/3197*z1)^6; +s(51) = 1/2*c0*(-72357/12788+6250/3197*z1)^6-18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6-2* ... +z1+1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6-1/2*d0*(-72357/12788+6250/3197*z1)^6+18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5-292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6; +s(52) = d5*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(53) = d4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(54) = d4*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(55) = d3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(56) = d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(57) = d3*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(58) = d2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(59) = d2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(60) = d2*(-72357/12788+6250/3197*z1)^4; +s(61) = d1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(62) = d1*(-72357/12788+6250/3197*z1)^4; +s(63) = d0*(-72357/12788+6250/3197*z1)^4; +s(64) = 1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6-1/2*d0*(-72357/12788+6250/3197*z1)^6+18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5-292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6-2* ... +z1-1/2*c0*(-72357/12788+6250/3197*z1)^6+18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6; +s(65) = c5*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(66) = c4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(67) = c4*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(68) = c3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(69) = c3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(70) = c3*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(71) = c2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(72) = c2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(73) = c2*(-72357/12788+6250/3197*z1)^4; +s(74) = c1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(75) = c1*(-72357/12788+6250/3197*z1)^4; +s(76) = c0*(-72357/12788+6250/3197*z1)^4; +s(77) = -37500/3197*c0*(-72357/12788+6250/3197*z1)^5+37500/3197*c1* ... +(-72357/12788+6250/3197*z1)^5+1171875000/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1171875000/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-14648437500000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+14648437500000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+91552734375000000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-91552734375000000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-286102294921875000000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+286102294921875000000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+357627868652343750000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5; +s(78) = -2-178813934326171875000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5-18750/3197*c0*(-72357/12788+6250/3197*z1)^5+18750/3197* ... +c1*(-72357/12788+6250/3197*z1)^5+18750/3197*b0*(-72357/12788+6250/3197* ... +z1)^5-18750/3197*b1*(-72357/12788+6250/3197*z1)^5-585937500/10220809*b1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+585937500/10220809*b2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*b2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +b3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+178813934326171875000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5+45776367187500000/104464936614481*d3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-45776367187500000/104464936614481*d4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-143051147460937500000/333974402356495757*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+143051147460937500000/333974402356495757*d5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)+18750/3197*d1* ... +(-72357/12788+6250/3197*z1)^5-18750/3197*d0*(-72357/12788+6250/3197* ... +z1)^5-178813934326171875000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-178813934326171875000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5-585937500/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4+585937500/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-7324218750000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+7324218750000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+45776367187500000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-45776367187500000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-143051147460937500000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+143051147460937500000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+45776367187500000/104464936614481*b4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+143051147460937500000/333974402356495757*b4* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)-143051147460937500000/333974402356495757*b5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-45776367187500000/104464936614481*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2-585937500/10220809*d2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+585937500/10220809*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4-7324218750000/32675926373*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+7324218750000/32675926373* ... +d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(79) = a1*(-72357/12788+6250/3197*z1)^5; +s(80) = a1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(81) = a0*(-72357/12788+6250/3197*z1)^5; +s(82) = a3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(83) = a2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(84) = a2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4; +s(85) = a5*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(86) = a4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1); +s(87) = a4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(88) = a3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2; +s(89) = -1+18750/3197*c0*(-72357/12788+6250/3197*z1)^5-18750/3197*c1* ... +(-72357/12788+6250/3197* ... +z1)^5-178813934326171875000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5+585937500/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-585937500/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-45776367187500000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+45776367187500000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+143051147460937500000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-143051147460937500000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)-357627868652343750000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5+357627868652343750000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5-91552734375000000/104464936614481*a3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+91552734375000000/104464936614481*a4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+286102294921875000000/333974402356495757*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-286102294921875000000/333974402356495757*a5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)+1171875000/10220809*a2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+14648437500000/32675926373* ... +a2*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-14648437500000/32675926373*a3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+37500/3197*a0*(-72357/12788+6250/3197* ... +z1)^5-1171875000/10220809*a1*(z1-17029/5000)*(-72357/12788+6250/3197* ... +z1)^4-37500/3197*a1*(-72357/12788+6250/3197*z1)^5; +s(90) = a5*(z1-17029/5000)^5*(-72357/12788+6250/3197*z1); +s(91) = a4*(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)^2; +s(92) = a3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^3; +s(93) = a2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4; +s(94) = a1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5; +s(95) = a0*(-72357/12788+6250/3197*z1)^6; +s(96) = a0*(-72357/12788+6250/3197*z1)^6-37500/3197*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+585937500/10220809*a2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-4882812500000/32675926373*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+22888183593750000/104464936614481*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-57220458984375000000/333974402356495757*a5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+59604644775390625000000/1067716164333716935129*a0* ... +(z1-17029/5000)^6+1/2*c0*(-72357/12788+6250/3197*z1)^6-18750/3197*c1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5+292968750/10220809*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373* ... +c3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6-z1; +s(97) = -1-18750/3197*c0*(-72357/12788+6250/3197*z1)^5+18750/3197*c1* ... +(-72357/12788+6250/3197* ... +z1)^5+178813934326171875000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-178813934326171875000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5-585937500/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4+585937500/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-7324218750000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+7324218750000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+45776367187500000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-45776367187500000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-143051147460937500000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+143051147460937500000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)-357627868652343750000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5+357627868652343750000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5-91552734375000000/104464936614481*a3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+91552734375000000/104464936614481*a4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+286102294921875000000/333974402356495757*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-286102294921875000000/333974402356495757*a5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)+1171875000/10220809*a2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+14648437500000/32675926373* ... +a2*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-14648437500000/32675926373*a3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+37500/3197*a0*(-72357/12788+6250/3197* ... +z1)^5-1171875000/10220809*a1*(z1-17029/5000)*(-72357/12788+6250/3197* ... +z1)^4-37500/3197*a1*(-72357/12788+6250/3197*z1)^5; +s(98) = a0*(-72357/12788+6250/3197*z1)^6-37500/3197*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+585937500/10220809*a2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-4882812500000/32675926373*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+22888183593750000/104464936614481*a4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-57220458984375000000/333974402356495757*a5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+59604644775390625000000/1067716164333716935129*a0* ... +(z1-17029/5000)^6-1/2*c0*(-72357/12788+6250/3197*z1)^6+18750/3197*c1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5-292968750/10220809*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373* ... +c3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6-z1; +s(99) = -37500/3197*d0*(-72357/12788+6250/3197*z1)^5+37500/3197*d1* ... +(-72357/12788+6250/3197*z1)^5+1171875000/10220809*d1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1171875000/10220809*d2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-14648437500000/32675926373*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+14648437500000/32675926373* ... +d3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+91552734375000000/104464936614481*d3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-91552734375000000/104464936614481*d4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-286102294921875000000/333974402356495757*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+286102294921875000000/333974402356495757*d5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+357627868652343750000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-357627868652343750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5; +s(100) = d0*(-72357/12788+6250/3197*z1)^6-37500/3197*d1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+585937500/10220809*d2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-4882812500000/32675926373*d3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+22888183593750000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-57220458984375000000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+59604644775390625000000/1067716164333716935129*c0*(z1-17029/5000)^6; +s(101) = b5*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(102) = b4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(103) = b4*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(104) = b3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(105) = b3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(106) = b3*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(107) = b2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(108) = b1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(109) = a0*(-72357/12788+6250/3197*z1)^4; +s(110) = b2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(111) = b2*(-72357/12788+6250/3197*z1)^4; +s(112) = b1*(-72357/12788+6250/3197*z1)^4; +s(113) = b0*(-72357/12788+6250/3197*z1)^4; +s(114) = a1*(-72357/12788+6250/3197*z1)^4; +s(115) = a5*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(116) = a4*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(117) = a4*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(118) = a3*(z1-17029/5000)^3*(-72357/12788+6250/3197*z1); +s(119) = a3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(120) = a3*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(121) = a2*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(122) = a1*(z1-17029/5000)*(-72357/12788+6250/3197*z1)^3; +s(123) = a2*(-72357/12788+6250/3197*z1)^4; +s(124) = -2-178813934326171875000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5+18750/3197*c0*(-72357/12788+6250/3197*z1)^5-18750/3197* ... +c1*(-72357/12788+6250/3197*z1)^5+18750/3197*b0*(-72357/12788+6250/3197* ... +z1)^5-18750/3197*b1*(-72357/12788+6250/3197*z1)^5-585937500/10220809*b1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+585937500/10220809*b2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*b2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +b3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-178813934326171875000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-45776367187500000/104464936614481*d3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+45776367187500000/104464936614481*d4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+143051147460937500000/333974402356495757*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-143051147460937500000/333974402356495757*d5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)-18750/3197*d1* ... +(-72357/12788+6250/3197*z1)^5+18750/3197*d0*(-72357/12788+6250/3197* ... +z1)^5+178813934326171875000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5-178813934326171875000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5+585937500/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-585937500/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-45776367187500000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+45776367187500000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+143051147460937500000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-143051147460937500000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+45776367187500000/104464936614481*b4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+143051147460937500000/333974402356495757*b4* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)-143051147460937500000/333974402356495757*b5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-45776367187500000/104464936614481*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2+585937500/10220809*d2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4-585937500/10220809*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(125) = 1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6+1/2*d0*(-72357/12788+6250/3197*z1)^6-18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5+292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6-2* ... +z1+1/2*c0*(-72357/12788+6250/3197*z1)^6-18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6; +s(126) = -3075000/3197*c0*(-72357/12788+6250/3197*z1)^5+3075000/3197*c1* ... +(-72357/12788+6250/3197*z1)^5+96093750000/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-96093750000/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1201171875000000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+1201171875000000/32675926373*c3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+7507324218750000000/104464936614481*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-7507324218750000000/104464936614481*c4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-23460388183593750000000/333974402356495757* ... +c4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+23460388183593750000000/333974402356495757*c5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+29325485229492187500000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5; +s(127) = -3075000/3197*d0*(-72357/12788+6250/3197*z1)^5+3075000/3197*d1* ... +(-72357/12788+6250/3197*z1)^5+96093750000/10220809*d1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-96093750000/10220809*d2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1201171875000000/32675926373*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+1201171875000000/32675926373*d3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+7507324218750000000/104464936614481*d3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-7507324218750000000/104464936614481*d4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-23460388183593750000000/333974402356495757* ... +d4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+23460388183593750000000/333974402356495757*d5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+29325485229492187500000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5; +s(128) = -30000000/3197*c0*(-72357/12788+6250/3197*z1)^5+30000000/3197*c1* ... +(-72357/12788+6250/3197*z1)^5+937500000000/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-937500000000/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-11718750000000000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+11718750000000000/32675926373*c3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+73242187500000000000/104464936614481*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-73242187500000000000/104464936614481*c4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-228881835937500000000000/333974402356495757* ... +c4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+228881835937500000000000/333974402356495757*c5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+286102294921875000000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-286102294921875000000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5; +s(129) = -2-178813934326171875000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5-18750/3197*c0*(-72357/12788+6250/3197*z1)^5+18750/3197* ... +c1*(-72357/12788+6250/3197*z1)^5+18750/3197*b0*(-72357/12788+6250/3197* ... +z1)^5-18750/3197*b1*(-72357/12788+6250/3197*z1)^5-585937500/10220809*b1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+585937500/10220809*b2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*b2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +b3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3-178813934326171875000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-45776367187500000/104464936614481*d3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+45776367187500000/104464936614481*d4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2+143051147460937500000/333974402356495757*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-143051147460937500000/333974402356495757*d5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197*z1)-18750/3197*d1* ... +(-72357/12788+6250/3197*z1)^5+18750/3197*d0*(-72357/12788+6250/3197* ... +z1)^5+178813934326171875000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5+178813934326171875000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-178813934326171875000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5-585937500/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4+585937500/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-7324218750000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3+7324218750000/32675926373* ... +c3*(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+45776367187500000/104464936614481*c3*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-45776367187500000/104464936614481*c4* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-143051147460937500000/333974402356495757*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)+143051147460937500000/333974402356495757*c5* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+45776367187500000/104464936614481*b4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2+143051147460937500000/333974402356495757*b4* ... +(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)-143051147460937500000/333974402356495757*b5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)-45776367187500000/104464936614481*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197*z1)^2+585937500/10220809*d2* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4-585937500/10220809*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^4+7324218750000/32675926373*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3-7324218750000/32675926373* ... +d3*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^3; +s(130) = 1/2*b0*(-72357/12788+6250/3197*z1)^6-18750/3197*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5+292968750/10220809*b2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*b4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*b5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*b0* ... +(z1-17029/5000)^6+1/2*d0*(-72357/12788+6250/3197*z1)^6-18750/3197*d1* ... +(z1-17029/5000)*(-72357/12788+6250/3197*z1)^5+292968750/10220809*d2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^4-2441406250000/32675926373* ... +d3*(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3+11444091796875000/104464936614481*d4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2-28610229492187500000/333974402356495757*d5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)+29802322387695312500000/1067716164333716935129*c0*(z1-17029/5000)^6-2* ... +z1-1/2*c0*(-72357/12788+6250/3197*z1)^6+18750/3197*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^5-292968750/10220809*c2*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^4+2441406250000/32675926373*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^3-11444091796875000/104464936614481*c4*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197*z1)^2+28610229492187500000/333974402356495757*c5* ... +(z1-17029/5000)^5*(-72357/12788+6250/3197* ... +z1)-29802322387695312500000/1067716164333716935129*d0*(z1-17029/5000)^6; +s(131) = a2*(z1-17029/5000)^2*(-72357/12788+6250/3197*z1)^2; +s(132) = -3000000/3197*a0*(-72357/12788+6250/3197*z1)^5+3000000/3197*a1* ... +(-72357/12788+6250/3197*z1)^5+93750000000/10220809*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-93750000000/10220809*a2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1171875000000000/32675926373*a2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+1171875000000000/32675926373*a3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+7324218750000000000/104464936614481*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-7324218750000000000/104464936614481*a4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-22888183593750000000000/333974402356495757* ... +a4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+22888183593750000000000/333974402356495757*a5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+28610229492187500000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5-28610229492187500000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5; +s(133) = -1500000/3197*c0*(-72357/12788+6250/3197*z1)^5+1500000/3197*c1* ... +(-72357/12788+6250/3197*z1)^5+46875000000/10220809*c1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-46875000000/10220809*c2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-585937500000000/32675926373*c2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+585937500000000/32675926373*c3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+3662109375000000000/104464936614481*c3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-3662109375000000000/104464936614481*c4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-11444091796875000000000/333974402356495757* ... +c4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+11444091796875000000000/333974402356495757*c5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+14305114746093750000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5-14305114746093750000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5; +s(134) = -3075000/3197*b0*(-72357/12788+6250/3197*z1)^5+3075000/3197*b1* ... +(-72357/12788+6250/3197*z1)^5+96093750000/10220809*b1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-96093750000/10220809*b2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1201171875000000/32675926373*b2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+1201171875000000/32675926373*b3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+7507324218750000000/104464936614481*b3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-7507324218750000000/104464936614481*b4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-23460388183593750000000/333974402356495757* ... +b4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+23460388183593750000000/333974402356495757*b5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+29325485229492187500000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-29325485229492187500000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5; +s(135) = -60000000/3197*a0*(-72357/12788+6250/3197*z1)^5+60000000/3197*a1* ... +(-72357/12788+6250/3197*z1)^5+1875000000000/10220809*a1*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-1875000000000/10220809*a2*(z1-17029/5000)* ... +(-72357/12788+6250/3197*z1)^4-23437500000000000/32675926373*a2* ... +(z1-17029/5000)^2*(-72357/12788+6250/3197* ... +z1)^3+23437500000000000/32675926373*a3*(z1-17029/5000)^2* ... +(-72357/12788+6250/3197*z1)^3+146484375000000000000/104464936614481*a3* ... +(z1-17029/5000)^3*(-72357/12788+6250/3197* ... +z1)^2-146484375000000000000/104464936614481*a4*(z1-17029/5000)^3* ... +(-72357/12788+6250/3197*z1)^2-457763671875000000000000/333974402356495757* ... +a4*(z1-17029/5000)^4*(-72357/12788+6250/3197* ... +z1)+457763671875000000000000/333974402356495757*a5*(z1-17029/5000)^4* ... +(-72357/12788+6250/3197* ... +z1)+572204589843750000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5-572204589843750000000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5; + +jac_alpha = -1000/(-83250000/3197*s(79)-203247070312500000000/104464936614481* ... +s(88)+635147094726562500000000/333974402356495757* ... +s(86)-635147094726562500000000/333974402356495757* ... +s(85)+2601562500000/10220809*s(84)+32519531250000000/32675926373* ... +s(83)-2601562500000/10220809*s(80)-32519531250000000/32675926373* ... +s(82)+203247070312500000000/104464936614481*s(87)+83250000/3197* ... +s(81)+39459228515625000000/104464936614481* ... +s(3)-131893157958984375000000/333974402356495757*s(16)+17287500/3197* ... +s(31)+12412500/3197*s(25)-12412500/3197*s(24)+6752929687500000/32675926373* ... +s(21)-42205810546875000000/104464936614481* ... +s(19)+131893157958984375000000/333974402356495757* ... +s(17)-6752929687500000/32675926373* ... +s(20)+42205810546875000000/104464936614481*s(18)+540234375000/10220809* ... +s(23)+94699859619140625000000/333974402356495757*s(26)+478*cos(s(51))* ... +s(11)-478*cos(s(64))*s(11)+30303955078125000000/104464936614481* ... +s(29)+387890625000/10220809*s(14)-540234375000/10220809* ... +s(22)+10560+123310089111328125000000/333974402356495757* ... +s(2)-154137611389160156250000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5+154137611389160156250000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5-6313476562500000/32675926373* ... +s(5)+6313476562500000/32675926373* ... +s(6)+118374824523925781250000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5-80* ... +cos(s(98))-118374824523925781250000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5-164866447448730468750000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5+164866447448730468750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5+505078125000/10220809* ... +s(7)-793933868408203125000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^5+793933868408203125000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^5+4848632812500000/32675926373* ... +s(12)-39459228515625000000/104464936614481*s(4)+(-6150000/3197* ... +s(10)+6150000/3197*s(9)+192187500000/10220809*s(8)-192187500000/10220809* ... +s(7)-2402343750000000/32675926373*s(6)+2402343750000000/32675926373* ... +s(5)+15014648437500000000/104464936614481* ... +s(4)-15014648437500000000/104464936614481* ... +s(3)-46920776367187500000000/333974402356495757* ... +s(2)+46920776367187500000000/333974402356495757* ... +s(1)+58650970458984375000000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-58650970458984375000000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)*cos(s(100))-94699859619140625000000/333974402356495757* ... +s(27)+328*cos(s(100))-123310089111328125000000/333974402356495757* ... +s(1)-4848632812500000/32675926373*s(13)-387890625000/10220809*s(15)-478* ... +cos(s(64))*s(77)-17287500/3197*s(30)+16162500/3197*s(10)-16162500/3197* ... +s(9)-30303955078125000000/104464936614481*s(28)-505078125000/10220809* ... +s(8)+s(127)*cos(s(130))+80*cos(s(96))+478*cos(s(64))*s(99)+1600* ... +sin(s(98))-10888*cos(s(50)-37500/3197*s(49)+585937500/10220809* ... +s(48)-4882812500000/32675926373*s(47)+22888183593750000/104464936614481* ... +s(46)-57220458984375000000/333974402356495757* ... +s(45)+59604644775390625000000/1067716164333716935129*d0* ... +(z1-17029/5000)^6)-478*cos(s(51))*s(99)-478*cos(s(51))*s(77)+s(135)* ... +sin(s(96))-s(128)*sin(s(96))-s(132)*cos(s(96))+s(133)*cos(s(96))+s(126)* ... +cos(s(130))-s(135)*sin(s(98))-s(128)*sin(s(98))+s(133)*cos(s(98))+s(132)* ... +cos(s(98))-s(134)*cos(s(125))-s(127)*cos(s(125))+s(126)*cos(s(125))-1600* ... +sin(s(96))+s(134)*cos(s(130)))^2*(126617431640625000000/104464936614481* ... +s(71)+1055145263671875000000000/333974402356495757* ... +s(66)+126617431640625000000/104464936614481*s(67)-387890625000/10220809* ... +s(63)+540234375000/10220809*s(76)+775781250000/10220809* ... +s(62)-13505859375000000/32675926373*s(74)+540234375000/10220809* ... +s(73)-527572631835937500000000/333974402356495757* ... +s(65)+27011718750000000/32675926373* ... +s(72)+378799438476562500000000/333974402356495757* ... +s(55)-493240356445312500000000/333974402356495757* ... +s(101)+118377685546875000000/104464936614481* ... +s(103)-12626953125000000/32675926373* ... +s(106)-493240356445312500000000/333974402356495757* ... +s(104)+118377685546875000000/104464936614481* ... +s(107)-12626953125000000/32675926373*s(108)+25253906250000000/32675926373* ... +s(110)+2601562500000/10220809*s(109)+505078125000/10220809* ... +s(113)-2540588378906250000000000/333974402356495757* ... +s(115)-5203125000000/10220809* ... +s(114)+5081176757812500000000000/333974402356495757* ... +s(116)-2540588378906250000000000/333974402356495757* ... +s(118)+609741210937500000000/104464936614481* ... +s(117)-1219482421875000000000/104464936614481* ... +s(119)-65039062500000000/32675926373*s(120)+2601562500000/10220809* ... +s(123)+609741210937500000000/104464936614481* ... +s(131)+130078125000000000/32675926373*s(121)-1010156250000/10220809* ... +s(112)-65039062500000000/32675926373*s(122)+505078125000/10220809* ... +s(111)+986480712890625000000000/333974402356495757* ... +s(102)-757598876953125000000000/333974402356495757* ... +s(53)+9697265625000000/32675926373* ... +s(57)-90911865234375000000/104464936614481*s(54)-387890625000/10220809* ... +s(60)-19394531250000000/32675926373* ... +s(59)+181823730468750000000/104464936614481* ... +s(56)-253234863281250000000/104464936614481*s(69)-1080468750000/10220809* ... +s(75)-527572631835937500000000/333974402356495757* ... +s(68)-236755371093750000000/104464936614481* ... +s(105)-13505859375000000/32675926373*s(70)-478*sin(s(51))*s(32)*s(11)+478* ... +sin(s(64))*s(78)*s(11)+10888*sin(s(50)-37500/3197*s(49)+585937500/10220809* ... +s(48)-4882812500000/32675926373*s(47)+22888183593750000/104464936614481* ... +s(46)-57220458984375000000/333974402356495757* ... +s(45)+59604644775390625000000/1067716164333716935129*d0*(z1-17029/5000)^6)* ... +(37500/3197*s(31)-37500/3197*s(30)-1171875000/10220809* ... +s(22)+1171875000/10220809*s(23)+14648437500000/32675926373* ... +s(21)-14648437500000/32675926373*s(20)-91552734375000000/104464936614481* ... +s(19)+91552734375000000/104464936614481* ... +s(18)+286102294921875000000/333974402356495757* ... +s(17)-286102294921875000000/333974402356495757* ... +s(16)-357627868652343750000000/1067716164333716935129*c5* ... +(z1-17029/5000)^5+357627868652343750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^5)+478*sin(s(51))*s(32)*s(99)+478*sin(s(51))*s(32)* ... +s(77)+9697265625000000/32675926373*s(61)-328*sin(s(100))*(37500/3197* ... +s(24)-37500/3197*s(25)-1171875000/10220809*s(14)+1171875000/10220809* ... +s(15)+14648437500000/32675926373*s(13)-14648437500000/32675926373* ... +s(12)-91552734375000000/104464936614481* ... +s(29)+91552734375000000/104464936614481* ... +s(28)+286102294921875000000/333974402356495757* ... +s(27)-286102294921875000000/333974402356495757* ... +s(26)-357627868652343750000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5+357627868652343750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5)-(-6150000/3197*s(10)+6150000/3197* ... +s(9)+192187500000/10220809*s(8)-192187500000/10220809* ... +s(7)-2402343750000000/32675926373*s(6)+2402343750000000/32675926373* ... +s(5)+15014648437500000000/104464936614481* ... +s(4)-15014648437500000000/104464936614481* ... +s(3)-46920776367187500000000/333974402356495757* ... +s(2)+46920776367187500000000/333974402356495757* ... +s(1)+58650970458984375000000000/1067716164333716935129*b5* ... +(z1-17029/5000)^5-58650970458984375000000000/1067716164333716935129*b0* ... +(z1-17029/5000)^5)*sin(s(100))*(37500/3197*s(24)-37500/3197* ... +s(25)-1171875000/10220809*s(14)+1171875000/10220809* ... +s(15)+14648437500000/32675926373*s(13)-14648437500000/32675926373* ... +s(12)-91552734375000000/104464936614481* ... +s(29)+91552734375000000/104464936614481* ... +s(28)+286102294921875000000/333974402356495757* ... +s(27)-286102294921875000000/333974402356495757* ... +s(26)-357627868652343750000000/1067716164333716935129*d5* ... +(z1-17029/5000)^5+357627868652343750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^5)+378799438476562500000000/333974402356495757* ... +s(52)-90911865234375000000/104464936614481*s(58)-478*cos(s(64))* ... +(-1171875000/10220809*s(113)+2343750000/10220809* ... +s(112)+29296875000000/32675926373*s(108)-1171875000/10220809* ... +s(111)-58593750000000/32675926373* ... +s(110)-274658203125000000/104464936614481* ... +s(107)+29296875000000/32675926373* ... +s(106)+549316406250000000/104464936614481* ... +s(105)+1144409179687500000000/333974402356495757* ... +s(104)-274658203125000000/104464936614481* ... +s(103)-2288818359375000000000/333974402356495757* ... +s(102)-1788139343261718750000000/1067716164333716935129*b4* ... +(z1-17029/5000)^4+1144409179687500000000/333974402356495757* ... +s(101)+3576278686523437500000000/1067716164333716935129*b5* ... +(z1-17029/5000)^4-1788139343261718750000000/1067716164333716935129*b0* ... +(z1-17029/5000)^4)+(-46875000000/10220809*s(76)+93750000000/10220809* ... +s(75)+1171875000000000/32675926373*s(74)-46875000000/10220809* ... +s(73)-2343750000000000/32675926373* ... +s(72)-10986328125000000000/104464936614481* ... +s(71)+1171875000000000/32675926373* ... +s(70)+21972656250000000000/104464936614481* ... +s(69)+45776367187500000000000/333974402356495757* ... +s(68)-10986328125000000000/104464936614481* ... +s(67)-91552734375000000000000/333974402356495757* ... +s(66)-71525573730468750000000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+45776367187500000000000/333974402356495757* ... +s(65)+143051147460937500000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-71525573730468750000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)*cos(s(98))-(-937500000000/10220809* ... +s(76)+1875000000000/10220809*s(75)+23437500000000000/32675926373* ... +s(74)-937500000000/10220809*s(73)-46875000000000000/32675926373* ... +s(72)-219726562500000000000/104464936614481* ... +s(71)+23437500000000000/32675926373* ... +s(70)+439453125000000000000/104464936614481* ... +s(69)+915527343750000000000000/333974402356495757* ... +s(68)-219726562500000000000/104464936614481* ... +s(67)-1831054687500000000000000/333974402356495757* ... +s(66)-1430511474609375000000000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+915527343750000000000000/333974402356495757* ... +s(65)+2861022949218750000000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-1430511474609375000000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)*sin(s(98))+(-46875000000/10220809* ... +s(76)+93750000000/10220809*s(75)+1171875000000000/32675926373* ... +s(74)-46875000000/10220809*s(73)-2343750000000000/32675926373* ... +s(72)-10986328125000000000/104464936614481* ... +s(71)+1171875000000000/32675926373* ... +s(70)+21972656250000000000/104464936614481* ... +s(69)+45776367187500000000000/333974402356495757* ... +s(68)-10986328125000000000/104464936614481* ... +s(67)-91552734375000000000000/333974402356495757* ... +s(66)-71525573730468750000000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+45776367187500000000000/333974402356495757* ... +s(65)+143051147460937500000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-71525573730468750000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)*cos(s(96))-(-937500000000/10220809* ... +s(76)+1875000000000/10220809*s(75)+23437500000000000/32675926373* ... +s(74)-937500000000/10220809*s(73)-46875000000000000/32675926373* ... +s(72)-219726562500000000000/104464936614481* ... +s(71)+23437500000000000/32675926373* ... +s(70)+439453125000000000000/104464936614481* ... +s(69)+915527343750000000000000/333974402356495757* ... +s(68)-219726562500000000000/104464936614481* ... +s(67)-1831054687500000000000000/333974402356495757* ... +s(66)-1430511474609375000000000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+915527343750000000000000/333974402356495757* ... +s(65)+2861022949218750000000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-1430511474609375000000000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)*sin(s(96))+(-96093750000/10220809* ... +s(76)+192187500000/10220809*s(75)+2402343750000000/32675926373* ... +s(74)-96093750000/10220809*s(73)-4804687500000000/32675926373* ... +s(72)-22521972656250000000/104464936614481* ... +s(71)+2402343750000000/32675926373* ... +s(70)+45043945312500000000/104464936614481* ... +s(69)+93841552734375000000000/333974402356495757* ... +s(68)-22521972656250000000/104464936614481* ... +s(67)-187683105468750000000000/333974402356495757* ... +s(66)-146627426147460937500000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+93841552734375000000000/333974402356495757* ... +s(65)+293254852294921875000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-146627426147460937500000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)*cos(s(130))+(-96093750000/10220809* ... +s(76)+192187500000/10220809*s(75)+2402343750000000/32675926373* ... +s(74)-96093750000/10220809*s(73)-4804687500000000/32675926373* ... +s(72)-22521972656250000000/104464936614481* ... +s(71)+2402343750000000/32675926373* ... +s(70)+45043945312500000000/104464936614481* ... +s(69)+93841552734375000000000/333974402356495757* ... +s(68)-22521972656250000000/104464936614481* ... +s(67)-187683105468750000000000/333974402356495757* ... +s(66)-146627426147460937500000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+93841552734375000000000/333974402356495757* ... +s(65)+293254852294921875000000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-146627426147460937500000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)*cos(s(125))-478*sin(s(64))*s(78)*s(99)-478*cos(s(64))* ... +(-1171875000/10220809*s(76)+2343750000/10220809* ... +s(75)+29296875000000/32675926373*s(74)-1171875000/10220809* ... +s(73)-58593750000000/32675926373*s(72)-274658203125000000/104464936614481* ... +s(71)+29296875000000/32675926373*s(70)+549316406250000000/104464936614481* ... +s(69)+1144409179687500000000/333974402356495757* ... +s(68)-274658203125000000/104464936614481* ... +s(67)-2288818359375000000000/333974402356495757* ... +s(66)-1788139343261718750000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+1144409179687500000000/333974402356495757* ... +s(65)+3576278686523437500000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-1788139343261718750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)+478*sin(s(64))*s(78)*s(77)+478*cos(s(51))* ... +(-1171875000/10220809*s(113)+2343750000/10220809* ... +s(112)+29296875000000/32675926373*s(108)-1171875000/10220809* ... +s(111)-58593750000000/32675926373* ... +s(110)-274658203125000000/104464936614481* ... +s(107)+29296875000000/32675926373* ... +s(106)+549316406250000000/104464936614481* ... +s(105)+1144409179687500000000/333974402356495757* ... +s(104)-274658203125000000/104464936614481* ... +s(103)-2288818359375000000000/333974402356495757* ... +s(102)-1788139343261718750000000/1067716164333716935129*b4* ... +(z1-17029/5000)^4+1144409179687500000000/333974402356495757* ... +s(101)+3576278686523437500000000/1067716164333716935129*b5* ... +(z1-17029/5000)^4-1788139343261718750000000/1067716164333716935129*b0* ... +(z1-17029/5000)^4)-478*cos(s(51))*(-1171875000/10220809* ... +s(63)+2343750000/10220809*s(62)+29296875000000/32675926373* ... +s(61)-1171875000/10220809*s(60)-58593750000000/32675926373* ... +s(59)-274658203125000000/104464936614481*s(58)+29296875000000/32675926373* ... +s(57)+549316406250000000/104464936614481* ... +s(56)+1144409179687500000000/333974402356495757* ... +s(55)-274658203125000000/104464936614481* ... +s(54)-2288818359375000000000/333974402356495757* ... +s(53)-1788139343261718750000000/1067716164333716935129*d4* ... +(z1-17029/5000)^4+1144409179687500000000/333974402356495757* ... +s(52)+3576278686523437500000000/1067716164333716935129*d5* ... +(z1-17029/5000)^4-1788139343261718750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^4)-478*cos(s(51))*(-1171875000/10220809* ... +s(76)+2343750000/10220809*s(75)+29296875000000/32675926373* ... +s(74)-1171875000/10220809*s(73)-58593750000000/32675926373* ... +s(72)-274658203125000000/104464936614481*s(71)+29296875000000/32675926373* ... +s(70)+549316406250000000/104464936614481* ... +s(69)+1144409179687500000000/333974402356495757* ... +s(68)-274658203125000000/104464936614481* ... +s(67)-2288818359375000000000/333974402356495757* ... +s(66)-1788139343261718750000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4+1144409179687500000000/333974402356495757* ... +s(65)+3576278686523437500000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4-1788139343261718750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4)-s(133)*sin(s(96))*s(89)-s(128)*cos(s(96))*s(89)+s(132)* ... +sin(s(96))*s(89)-80*sin(s(96))*s(89)-1600*cos(s(96))*s(89)+s(135)* ... +cos(s(96))*s(89)-s(133)*sin(s(98))*s(97)-s(128)*cos(s(98))*s(97)-s(132)* ... +sin(s(98))*s(97)+80*sin(s(98))*s(97)+1600*cos(s(98))*s(97)-s(135)* ... +cos(s(98))*s(97)-591874122619628906250000000/1067716164333716935129*d4* ... +(z1-17029/5000)^4+1183748245239257812500000000/1067716164333716935129*d5* ... +(z1-17029/5000)^4-591874122619628906250000000/1067716164333716935129*c0* ... +(z1-17029/5000)^4+3969669342041015625000000000/1067716164333716935129*a4* ... +(z1-17029/5000)^4-7939338684082031250000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^4+3969669342041015625000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^4+770688056945800781250000000/1067716164333716935129*b4* ... +(z1-17029/5000)^4-1541376113891601562500000000/1067716164333716935129*b5* ... +(z1-17029/5000)^4+770688056945800781250000000/1067716164333716935129*b0* ... +(z1-17029/5000)^4+824332237243652343750000000/1067716164333716935129*c4* ... +(z1-17029/5000)^4-1648664474487304687500000000/1067716164333716935129*c5* ... +(z1-17029/5000)^4+824332237243652343750000000/1067716164333716935129*d0* ... +(z1-17029/5000)^4+(-96093750000/10220809*s(63)+192187500000/10220809* ... +s(62)+2402343750000000/32675926373*s(61)-96093750000/10220809* ... +s(60)-4804687500000000/32675926373* ... +s(59)-22521972656250000000/104464936614481* ... +s(58)+2402343750000000/32675926373* ... +s(57)+45043945312500000000/104464936614481* ... +s(56)+93841552734375000000000/333974402356495757* ... +s(55)-22521972656250000000/104464936614481* ... +s(54)-187683105468750000000000/333974402356495757* ... +s(53)-146627426147460937500000000/1067716164333716935129*d4* ... +(z1-17029/5000)^4+93841552734375000000000/333974402356495757* ... +s(52)+293254852294921875000000000/1067716164333716935129*d5* ... +(z1-17029/5000)^4-146627426147460937500000000/1067716164333716935129*c0* ... +(z1-17029/5000)^4)*cos(s(130))+478*cos(s(64))*(-1171875000/10220809* ... +s(63)+2343750000/10220809*s(62)+29296875000000/32675926373* ... +s(61)-1171875000/10220809*s(60)-58593750000000/32675926373* ... +s(59)-274658203125000000/104464936614481*s(58)+29296875000000/32675926373* ... +s(57)+549316406250000000/104464936614481* ... +s(56)+1144409179687500000000/333974402356495757* ... +s(55)-274658203125000000/104464936614481* ... +s(54)-2288818359375000000000/333974402356495757* ... +s(53)-1788139343261718750000000/1067716164333716935129*d4* ... +(z1-17029/5000)^4+1144409179687500000000/333974402356495757* ... +s(52)+3576278686523437500000000/1067716164333716935129*d5* ... +(z1-17029/5000)^4-1788139343261718750000000/1067716164333716935129*c0* ... +(z1-17029/5000)^4)-(-96093750000/10220809*s(63)+192187500000/10220809* ... +s(62)+2402343750000000/32675926373*s(61)-96093750000/10220809* ... +s(60)-4804687500000000/32675926373* ... +s(59)-22521972656250000000/104464936614481* ... +s(58)+2402343750000000/32675926373* ... +s(57)+45043945312500000000/104464936614481* ... +s(56)+93841552734375000000000/333974402356495757* ... +s(55)-22521972656250000000/104464936614481* ... +s(54)-187683105468750000000000/333974402356495757* ... +s(53)-146627426147460937500000000/1067716164333716935129*d4* ... +(z1-17029/5000)^4+93841552734375000000000/333974402356495757* ... +s(52)+293254852294921875000000000/1067716164333716935129*d5* ... +(z1-17029/5000)^4-146627426147460937500000000/1067716164333716935129*c0* ... +(z1-17029/5000)^4)*cos(s(125))+(-1875000000000/10220809* ... +s(109)+3750000000000/10220809*s(114)+46875000000000000/32675926373* ... +s(122)-1875000000000/10220809*s(123)-93750000000000000/32675926373* ... +s(121)-439453125000000000000/104464936614481* ... +s(131)+46875000000000000/32675926373* ... +s(120)+878906250000000000000/104464936614481* ... +s(119)+1831054687500000000000000/333974402356495757* ... +s(118)-439453125000000000000/104464936614481* ... +s(117)-3662109375000000000000000/333974402356495757* ... +s(116)-2861022949218750000000000000/1067716164333716935129*a4* ... +(z1-17029/5000)^4+1831054687500000000000000/333974402356495757* ... +s(115)+5722045898437500000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^4-2861022949218750000000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^4)*sin(s(96))-(-93750000000/10220809* ... +s(109)+187500000000/10220809*s(114)+2343750000000000/32675926373* ... +s(122)-93750000000/10220809*s(123)-4687500000000000/32675926373* ... +s(121)-21972656250000000000/104464936614481* ... +s(131)+2343750000000000/32675926373* ... +s(120)+43945312500000000000/104464936614481* ... +s(119)+91552734375000000000000/333974402356495757* ... +s(118)-21972656250000000000/104464936614481* ... +s(117)-183105468750000000000000/333974402356495757* ... +s(116)-143051147460937500000000000/1067716164333716935129*a4* ... +(z1-17029/5000)^4+91552734375000000000000/333974402356495757* ... +s(115)+286102294921875000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^4-143051147460937500000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^4)*cos(s(96))+s(127)*sin(s(125))* ... +s(124)+(-93750000000/10220809*s(109)+187500000000/10220809* ... +s(114)+2343750000000000/32675926373*s(122)-93750000000/10220809* ... +s(123)-4687500000000000/32675926373* ... +s(121)-21972656250000000000/104464936614481* ... +s(131)+2343750000000000/32675926373* ... +s(120)+43945312500000000000/104464936614481* ... +s(119)+91552734375000000000000/333974402356495757* ... +s(118)-21972656250000000000/104464936614481* ... +s(117)-183105468750000000000000/333974402356495757* ... +s(116)-143051147460937500000000000/1067716164333716935129*a4* ... +(z1-17029/5000)^4+91552734375000000000000/333974402356495757* ... +s(115)+286102294921875000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^4-143051147460937500000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^4)*cos(s(98))-(-1875000000000/10220809* ... +s(109)+3750000000000/10220809*s(114)+46875000000000000/32675926373* ... +s(122)-1875000000000/10220809*s(123)-93750000000000000/32675926373* ... +s(121)-439453125000000000000/104464936614481* ... +s(131)+46875000000000000/32675926373* ... +s(120)+878906250000000000000/104464936614481* ... +s(119)+1831054687500000000000000/333974402356495757* ... +s(118)-439453125000000000000/104464936614481* ... +s(117)-3662109375000000000000000/333974402356495757* ... +s(116)-2861022949218750000000000000/1067716164333716935129*a4* ... +(z1-17029/5000)^4+1831054687500000000000000/333974402356495757* ... +s(115)+5722045898437500000000000000/1067716164333716935129*a5* ... +(z1-17029/5000)^4-2861022949218750000000000000/1067716164333716935129*a0* ... +(z1-17029/5000)^4)*sin(s(98))+(-192187500000/10220809* ... +s(113)+384375000000/10220809*s(112)+4804687500000000/32675926373* ... +s(108)-192187500000/10220809*s(111)-9609375000000000/32675926373* ... +s(110)-45043945312500000000/104464936614481* ... +s(107)+4804687500000000/32675926373* ... +s(106)+90087890625000000000/104464936614481* ... +s(105)+187683105468750000000000/333974402356495757* ... +s(104)-45043945312500000000/104464936614481* ... +s(103)-375366210937500000000000/333974402356495757* ... +s(102)-293254852294921875000000000/1067716164333716935129*b4* ... +(z1-17029/5000)^4+187683105468750000000000/333974402356495757* ... +s(101)+586509704589843750000000000/1067716164333716935129*b5* ... +(z1-17029/5000)^4-293254852294921875000000000/1067716164333716935129*b0* ... +(z1-17029/5000)^4)*cos(s(100))+(-96093750000/10220809* ... +s(113)+192187500000/10220809*s(112)+2402343750000000/32675926373* ... +s(108)-96093750000/10220809*s(111)-4804687500000000/32675926373* ... +s(110)-22521972656250000000/104464936614481* ... +s(107)+2402343750000000/32675926373* ... +s(106)+45043945312500000000/104464936614481* ... +s(105)+93841552734375000000000/333974402356495757* ... +s(104)-22521972656250000000/104464936614481* ... +s(103)-187683105468750000000000/333974402356495757* ... +s(102)-146627426147460937500000000/1067716164333716935129*b4* ... +(z1-17029/5000)^4+93841552734375000000000/333974402356495757* ... +s(101)+293254852294921875000000000/1067716164333716935129*b5* ... +(z1-17029/5000)^4-146627426147460937500000000/1067716164333716935129*b0* ... +(z1-17029/5000)^4)*cos(s(130))-(-96093750000/10220809* ... +s(113)+192187500000/10220809*s(112)+2402343750000000/32675926373* ... +s(108)-96093750000/10220809*s(111)-4804687500000000/32675926373* ... +s(110)-22521972656250000000/104464936614481* ... +s(107)+2402343750000000/32675926373* ... +s(106)+45043945312500000000/104464936614481* ... +s(105)+93841552734375000000000/333974402356495757* ... +s(104)-22521972656250000000/104464936614481* ... +s(103)-187683105468750000000000/333974402356495757* ... +s(102)-146627426147460937500000000/1067716164333716935129*b4* ... +(z1-17029/5000)^4+93841552734375000000000/333974402356495757* ... +s(101)+293254852294921875000000000/1067716164333716935129*b5* ... +(z1-17029/5000)^4-146627426147460937500000000/1067716164333716935129*b0* ... +(z1-17029/5000)^4)*cos(s(125))-s(126)*sin(s(125))*s(124)+s(134)* ... +sin(s(125))*s(124)-s(134)*sin(s(130))*s(129)-s(126)*sin(s(130))* ... +s(129)-s(127)*sin(s(130))*s(129)); + +% change to original z coordinates +% +% NOTE! z1 (in old coords) = z1 (in new coords) +z_old = [z1; 1/alpha*z2]; + +dz_old = zd(z_old); + +dz = [z2; jac_alpha/alpha*z2^2+alpha*dz_old(2)]; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/zd_ctrl_sens_plots_cc.m b/demos/rabbit/anim/rabbit_model/zd_ctrl_sens_plots_cc.m new file mode 100644 index 0000000..2026271 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_ctrl_sens_plots_cc.m @@ -0,0 +1,125 @@ +function zd_ctrl_sens_plots_cc +%ZD_CTRL_SENS_PLOTS_CC + +%Eric Westervelt +%1/23/01 + +global z OUT X_MIN X_MAX +global DIFF_VECT_FIELD_SCL DIFF_VECT_FIELD_SCL_TORSO +global OMEGA_MIN + +OMEGA_MIN=-1.5; + +DIFF_VECT_FIELD_SCL = 5; +DIFF_VECT_FIELD_SCL_TORSO = 1; + +X_MIN = 2.85; +X_MAX = 3.42; + +WIDTH = 800; +HEIGHT = 800; + +fig_hl = figure(2); +clf +subplot(5,5,1); +set(fig_hl,'Position', [100 50 WIDTH HEIGHT]); +set(fig_hl,'PaperPosition', [.25 .25 8 10.5]); + +[t,z] = sim_zd_cc; + +% display stats +x = zd_lift(z(1,1),z(1,2)); +sl = step_length(x); +[tmp,hh] = hip_pos(x); + +vectfield('zd_cc', ... + linspace(X_MIN,X_MAX,10), ... + linspace(OMEGA_MIN,0,10),1); +axis([X_MIN X_MAX OMEGA_MIN 0]); +hold on +h=plot(z(:,1),z(:,2),'r'); +set(h,'linewidth',1.5) +ylabel('z2 \rightarrow 1/2(dq_{31}+dq_{41})'); +title('zero dynamics vector field'); +drawnow; + +[OUT,nominal] = start_of_step_ctrl_sens(2); % specify METHOD here + +uicontrol('Position',[0 0 700 25], ... + 'HorizontalAlignment','left', ... + 'Style','text', ... + 'String',['step length = ', num2str(abs(sl)), ... + ', hip height = ', num2str(abs(hh)), ... + ', diff vect. field scl = ', ... + num2str(DIFF_VECT_FIELD_SCL), ... + ', diff vect. field scl_torso = ', ... + num2str(DIFF_VECT_FIELD_SCL_TORSO), ... + ', nominal = ', ... + num2str(nominal)]); + + +plot_stuff(1,'diff vector field a_0'); +plot_stuff(2,'diff vector field a_1'); +plot_stuff(3,'diff vector field a_2'); +plot_stuff(4,'diff vector field a_3'); +plot_stuff(5,'diff vector field a_4'); +plot_stuff(6,'diff vector field a_5'); + +plot_stuff(7,'diff vector field b_0'); +plot_stuff(8,'diff vector field b_1'); +plot_stuff(9,'diff vector field b_2'); +plot_stuff(10,'diff vector field b_3'); +plot_stuff(11,'diff vector field b_4'); +plot_stuff(12,'diff vector field b_5'); + +plot_stuff(13,'diff vector field c_0'); +plot_stuff(14,'diff vector field c_1'); +plot_stuff(15,'diff vector field c_2'); +plot_stuff(16,'diff vector field c_3'); +plot_stuff(17,'diff vector field c_4'); +plot_stuff(18,'diff vector field c_5'); + +plot_stuff(19,'diff vector field d_0'); +plot_stuff(20,'diff vector field d_1'); +plot_stuff(21,'diff vector field d_2'); +plot_stuff(22,'diff vector field d_3'); +plot_stuff(23,'diff vector field d_4'); +plot_stuff(24,'diff vector field d_5'); + +print plots/ctrl_sens.ps + + +%---------------------------------------------- +function plot_stuff(ctrl_param_indx,title_string) + +global z OUT X_MIN X_MAX +global DIFF_VECT_FIELD_SCL DIFF_VECT_FIELD_SCL_TORSO +global WHICH_TO_TWEAK OMEGA_MIN + +figure(2) + +subplot(5,5,ctrl_param_indx+1); +if ctrl_param_indx < 7 + vectfield('zd_diff_cc', ... + linspace(X_MIN,X_MAX,10), linspace(OMEGA_MIN,0,10), ... + DIFF_VECT_FIELD_SCL_TORSO,ctrl_param_indx); +else + vectfield('zd_diff_cc', ... + linspace(X_MIN,X_MAX,10), linspace(OMEGA_MIN,0,10), ... + DIFF_VECT_FIELD_SCL,ctrl_param_indx); +end +axis([X_MIN X_MAX OMEGA_MIN 0]); + +if ctrl_param_indx >= 20 + xlabel('z1 \rightarrow 1/2(q_{31}+q_{41})'); +end +if mod(ctrl_param_indx,5) == 0 + ylabel('z2 \rightarrow 1/2(dq_{31}+dq_{41})'); +end +title(title_string); +grid; drawnow; + +hold on +h=plot(z(:,1),z(:,2),'r'); +set(h,'linewidth',1.5) +text(X_MIN*1.01,-.2,num2str(OUT(ctrl_param_indx))); \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/zd_diff.m b/demos/rabbit/anim/rabbit_model/zd_diff.m new file mode 100644 index 0000000..63c8d30 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_diff.m @@ -0,0 +1,7751 @@ +function dz = zd_diff(z,opt) +%ZD_DIFF + +%Eric Westervelt +%27-Feb-2001 00:33:06 + +[a,b,m] = controlParameters; + +a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9); a5=a(10); +b0=a(11); b1=a(12); b2=a(13); b3=a(14); b4=a(15); b5=a(16); +c0=a(17); c1=a(18); c2=a(19); c3=a(20); c4=a(21); c5=a(22); +d0=a(23); d1=a(24); d2=a(25); d3=a(26); d4=a(27); d5=a(28); + +a0_0=a(5); a1_0=a(6); a2_0=a(7); a3_0=a(8); a4_0=a(9); a5_0=a(10); +b0_0=a(11); b1_0=a(12); b2_0=a(13); b3_0=a(14); b4_0=a(15); b5_0=a(16); +c0_0=a(17); c1_0=a(18); c2_0=a(19); c3_0=a(20); c4_0=a(21); c5_0=a(22); +d0_0=a(23); d1_0=a(24); d2_0=a(25); d3_0=a(26); d4_0=a(27); d5_0=a(28); + +z1 = z(1); z2 = z(2); + +switch opt + case 1, + % dz_partial_a0 (state one) + s(1) = -1.*(-5.658195183+1.954957773*z1)^6-55.82442859*(z1-3.405800000)^ ... +6; + s(2) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(3) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(4) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(5) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(6) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(7) = c0*(-5.658195183+1.954957773*z1)^6; + s(8) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(9) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(10) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(11) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(12) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(13) = a0_0*(-5.658195183+1.954957773*z1)^6; + s(14) = -1.*a0_0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0_0*(z1-3.405800000)^6+.5000000000* ... +c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(15) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(17) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(19) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(21) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(23) = a1*(-5.658195183+1.954957773*z1)^5; + s(24) = a0_0*(-5.658195183+1.954957773*z1)^5; + s(25) = -11.72974664*a0_0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716* ... +a0_0*(z1-3.405800000)^5; + s(26) = -11.72974664*(-5.658195183+1.954957773*z1)^5-334.9465716* ... +(z1-3.405800000)^5; + s(27) = -1.*a0_0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0_0*(z1-3.405800000)^6-.5000000000* ... +c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(28) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(29) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(30) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(31) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(32) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(33) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(34) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(35) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(36) = c1*(-5.658195183+1.954957773*z1)^5; + s(37) = c0*(-5.658195183+1.954957773*z1)^5; + s(38) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(39) = d0*(-5.658195183+1.954957773*z1)^5; + s(40) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = b1*(-5.658195183+1.954957773*z1)^5; + s(42) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(43) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(45) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(47) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(48) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(49) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(50) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(51) = d0*(-5.658195183+1.954957773*z1)^6; + s(52) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(53) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(55) = b0*(-5.658195183+1.954957773*z1)^5; + s(56) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(57) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(58) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(59) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(60) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(61) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(62) = b0*(-5.658195183+1.954957773*z1)^6; + s(63) = cos(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6); + s(64) = cos(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6); + s(65) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(66) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6); + s(67) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6); + s(68) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 1; + + dz(1) = -1000.*z2/(-5055.520801*s(41)-148385.4737*s(72)-193214.9219* ... +s(43)-290087.3351*s(69)-49416.64843*s(40)-37951.06875*s(73)+ ... +193214.9219*s(44)+283554.2453*s(65)+5055.520801* ... +s(55)-369220.1805*s(52)+148385.4737*s(71)+37951.06875* ... +s(74)-283554.2453*s(68)+369220.1805*s(53)+377727.0134* ... +s(54)-377727.0134*s(42)-3882.546137*s(39)+290087.3351*s(70)+ ... +3882.546137*s(75)+49416.64843*s(45)-26040.03754* ... +s(23)-1945600.858*s(18)+26040.03754*s(24)-5407.413200*s(36)+ ... +206663.7564*s(33)+404018.9169*s(30)-995213.7509* ... +s(19)-743581.3889*a5*(z1-3.405800000)^5+743581.3889*a0_0* ... +(z1-3.405800000)^5+254535.8689*s(21)+1901783.760* ... +s(16)-154410.3695*c5*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-144361.9723*b5*(z1-3.405800000)^5+144361.9723* ... +b0*(z1-3.405800000)^5+1600.*sin(s(27))-52856.32233*s(35)+80.* ... +s(25)*cos(s(14))+1600.*s(25)*sin(s(14))+40.*s(38)*cos(s(14))+ ... +800.*s(38)*sin(s(14))-80.*cos(s(14))-1600.*sin(s(14))+ ... +394919.9610*s(29)+995213.7509*s(20)-1901783.760* ... +s(15)-206663.7564*s(32)+800.*s(38)*sin(s(27))+40.*s(38)* ... +cos(s(27))+80.*cos(s(27))-394919.9610*s(28)+110867.3152*d5* ... +(z1-3.405800000)^5-110867.3152*c0*(z1-3.405800000)^5+10560.+328.* ... +cos(s(51)-11.72974664*s(50)+57.32789841*s(49)-149.4314941*s(48)+ ... +219.0991957*s(47)-171.3318703*s(46)+55.82442859*c0* ... +(z1-3.405800000)^6)-10888.*cos(s(7)-11.72974664*s(6)+57.32789841* ... +s(5)-149.4314941*s(4)+219.0991957*s(3)-171.3318703*s(2)+ ... +55.82442859*d0*(z1-3.405800000)^6)+164.*s(56)* ... +cos(s(51)-11.72974664*s(50)+57.32789841*s(49)-149.4314941*s(48)+ ... +219.0991957*s(47)-171.3318703*s(46)+55.82442859*c0* ... +(z1-3.405800000)^6)-254535.8689*s(22)+5407.413200*s(37)+ ... +52856.32233*s(34)+1945600.858*s(17)-404018.9169*s(31)-478.*s(63)* ... +s(38)+82.*s(38)*s(66)-478.*s(64)*s(38)+82.*s(38)*s(67)+82.*s(56)* ... +s(66)+478.*s(63)*s(56)-478.*s(64)*s(56)-82.*s(56)*s(67)+82.* ... +s(76)*s(66)-478.*s(63)*s(76)-80.*s(25)*cos(s(27))-1600.*s(25)* ... +sin(s(27))+478.*s(64)*s(76)-82.*s(76)*s(67))^2*(26040.03754* ... +(-5.658195183+1.954957773*z1)^5+743581.3889*(z1-3.405800000)^ ... +5-40.*s(38)*sin(s(14))*s(1)+800.*s(38)*cos(s(14))*s(1)+800.* ... +s(38)*cos(s(27))*s(1)-40.*s(38)*sin(s(27))*s(1)-80.*sin(s(27))* ... +s(1)+80.*sin(s(14))*s(1)+1600.*cos(s(27))*s(1)-1600.*cos(s(14))* ... +s(1)-80.*s(26)*cos(s(27))+80.*s(25)*sin(s(27))*s(1)+80.*s(26)* ... +cos(s(14))-80.*s(25)*sin(s(14))*s(1)-1600.*s(26)* ... +sin(s(27))-1600.*s(25)*cos(s(27))*s(1)+1600.*s(26)*sin(s(14))+ ... +1600.*s(25)*cos(s(14))*s(1)); + + + % dz_partial_a0 (state two) + dz(2) = 1; + + dz(2) = 1.962000000*sin(-1.*a0_0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0_0* ... +(z1-3.405800000)^6)*(-1.*(-5.658195183+1.954957773*z1)^ ... +6-55.82442859*(z1-3.405800000)^6)-39.24000000*cos(-1.*a0_0* ... +(-5.658195183+1.954957773*z1)^6+11.72974664*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-57.32789841*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+149.4314941*a3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-219.0991957* ... +a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-55.82442859*a0_0*(z1-3.405800000)^6)*(-1.*(-5.658195183+ ... +1.954957773*z1)^6-55.82442859*(z1-3.405800000)^6); + + case 2, + % dz_partial_a1 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = a1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = a0*(-5.658195183+1.954957773*z1)^6; + s(13) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(14) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(15) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(17) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(19) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = c1*(-5.658195183+1.954957773*z1)^5; + s(23) = c0*(-5.658195183+1.954957773*z1)^5; + s(24) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(25) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(26) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(27) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(28) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(29) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(30) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(31) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(32) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(33) = a1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(34) = a1_0*(-5.658195183+1.954957773*z1)^5; + s(35) = a0*(-5.658195183+1.954957773*z1)^5; + s(36) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1_0* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-114.6557968*a2* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+448.2944824* ... +a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(37) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(38) = 11.72974664*(-5.658195183+1.954957773*z1)^5+114.6557968* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(39) = d0*(-5.658195183+1.954957773*z1)^5; + s(40) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = b1*(-5.658195183+1.954957773*z1)^5; + s(42) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(43) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(45) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(47) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(48) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(49) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(50) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(51) = d0*(-5.658195183+1.954957773*z1)^6; + s(52) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(53) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(55) = b0*(-5.658195183+1.954957773*z1)^5; + s(56) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(57) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(58) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(59) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(60) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(61) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(62) = b0*(-5.658195183+1.954957773*z1)^6; + s(63) = cos(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6); + s(64) = cos(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6); + s(65) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(66) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6); + s(67) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6); + s(68) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 2; + + dz(1) = -1000.*z2/(1901783.760*s(27)+80.*cos(s(13))-394919.9610* ... +s(14)-110867.3152*c0*(z1-3.405800000)^5+110867.3152*d5* ... +(z1-3.405800000)^5-49416.64843*s(40)+369220.1805* ... +s(53)-148385.4737*s(72)+37951.06875*s(74)+290087.3351* ... +s(70)-283554.2453*s(68)+283554.2453*s(65)-3882.546137* ... +s(39)-37951.06875*s(73)+148385.4737*s(71)+377727.0134*s(54)+ ... +5055.520801*s(55)-290087.3351*s(69)-52856.32233*s(21)+ ... +404018.9169*s(16)-1945600.858*s(29)+995213.7509* ... +s(31)-254535.8689*s(33)-5407.413200*s(22)+52856.32233*s(20)+ ... +394919.9610*s(15)+26040.03754*s(35)-404018.9169* ... +s(17)-995213.7509*s(30)-1901783.760*s(26)-26040.03754*s(34)+ ... +1945600.858*s(28)+254535.8689*s(32)+49416.64843* ... +s(45)-5055.520801*s(41)-193214.9219*s(43)-377727.0134* ... +s(42)-369220.1805*s(52)-206663.7564*s(18)+5407.413200*s(23)+ ... +206663.7564*s(19)+3882.546137*s(75)+193214.9219*s(44)+478.*s(64)* ... +s(76)-82.*s(76)*s(67)+80.*s(36)*cos(s(25))+1600.*s(36)* ... +sin(s(25))-82.*s(56)*s(67)-478.*s(64)*s(56)+478.*s(63)*s(56)+82.* ... +s(76)*s(66)-478.*s(63)*s(76)-478.*s(63)*s(24)-478.*s(64)*s(24)+ ... +82.*s(24)*s(67)+82.*s(24)*s(66)+82.*s(56)*s(66)-1600.*s(36)* ... +sin(s(13))-80.*s(36)*cos(s(13))+800.*s(24)*sin(s(13))+40.*s(24)* ... +cos(s(13))+800.*s(24)*sin(s(25))+40.*s(24)*cos(s(25))+ ... +743581.3889*a0*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-10888.*cos(s(6)-11.72974664*s(5)+57.32789841* ... +s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703*s(1)+ ... +55.82442859*d0*(z1-3.405800000)^6)+10560.+164.*s(56)* ... +cos(s(51)-11.72974664*s(50)+57.32789841*s(49)-149.4314941*s(48)+ ... +219.0991957*s(47)-171.3318703*s(46)+55.82442859*c0* ... +(z1-3.405800000)^6)-743581.3889*a5*(z1-3.405800000)^ ... +5-154410.3695*c5*(z1-3.405800000)^5+328.*cos(s(51)-11.72974664* ... +s(50)+57.32789841*s(49)-149.4314941*s(48)+219.0991957* ... +s(47)-171.3318703*s(46)+55.82442859*c0*(z1-3.405800000)^ ... +6)-144361.9723*b5*(z1-3.405800000)^5+144361.9723*b0* ... +(z1-3.405800000)^5-80.*cos(s(25))-1600.*sin(s(25))+1600.* ... +sin(s(13)))^2*(-80.*s(38)*cos(s(13))+938.3797310*s(36)* ... +sin(s(13))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+80.* ... +s(38)*cos(s(25))-938.3797310*s(36)*sin(s(25))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-1600.*s(38)* ... +sin(s(13))-18767.59462*s(36)*cos(s(13))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-254535.8689*s(37)-26040.03754* ... +(-5.658195183+1.954957773*z1)^5+1600.*s(38)*sin(s(25))+ ... +18767.59462*s(36)*cos(s(25))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5-938.3797310*sin(s(13))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+938.3797310*sin(s(25))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+18767.59462* ... +cos(s(13))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-18767.59462*cos(s(25))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5-469.1898655*s(24)*sin(s(25))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-469.1898655*s(24)*sin(s(13))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+9383.797310* ... +s(24)*cos(s(25))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+9383.797310*s(24)*cos(s(13))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5); + + + % dz_partial_a1 (state two) + dz(2) = 2; + + dz(2) = 23.01376290*sin(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-460.2752581* ... +cos(-1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6)* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + + case 3, + % dz_partial_a2 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = a0*(-5.658195183+1.954957773*z1)^6; + s(13) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841* ... +a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(14) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(15) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(17) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(19) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = c1*(-5.658195183+1.954957773*z1)^5; + s(23) = c0*(-5.658195183+1.954957773*z1)^5; + s(24) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(25) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841* ... +a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(26) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(27) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(28) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(29) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(30) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(31) = a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(32) = a2_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(33) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(34) = a1*(-5.658195183+1.954957773*z1)^5; + s(35) = a0*(-5.658195183+1.954957773*z1)^5; + s(36) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824* ... +a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +448.2944824*a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +3+876.3967829*a3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^2-856.6593514*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^ ... +5-334.9465716*a0*(z1-3.405800000)^5; + s(37) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(38) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(39) = -114.6557968*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +4-448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = d0*(-5.658195183+1.954957773*z1)^5; + s(41) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = b1*(-5.658195183+1.954957773*z1)^5; + s(43) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(44) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(45) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(46) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(47) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(48) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(49) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(50) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(51) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(52) = d0*(-5.658195183+1.954957773*z1)^6; + s(53) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(55) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(56) = b0*(-5.658195183+1.954957773*z1)^5; + s(57) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(58) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(59) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(60) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(61) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(62) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(63) = b0*(-5.658195183+1.954957773*z1)^6; + s(64) = cos(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6); + s(65) = cos(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6); + s(66) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6); + s(68) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6); + s(69) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = d1*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 3; + + dz(1) = -1000.*z2/(49416.64843*s(46)+1901783.760*s(27)+80.* ... +cos(s(13))-394919.9610*s(14)+144361.9723*b0*(z1-3.405800000)^ ... +5-144361.9723*b5*(z1-3.405800000)^5-10888.*cos(s(6)-11.72974664* ... +s(5)+57.32789841*s(4)-149.4314941*s(3)+219.0991957* ... +s(2)-171.3318703*s(1)+55.82442859*d0*(z1-3.405800000)^6)+ ... +254535.8689*s(32)+206663.7564*s(19)+1945600.858* ... +s(28)-206663.7564*s(18)+5407.413200*s(23)-1901783.760*s(26)+ ... +394919.9610*s(15)+52856.32233*s(20)-26040.03754* ... +s(34)-369220.1805*s(53)-5055.520801*s(42)-193214.9219* ... +s(44)-49416.64843*s(41)+283554.2453*s(66)-3882.546137* ... +s(40)-290087.3351*s(70)-377727.0134*s(43)+5055.520801* ... +s(56)-148385.4737*s(73)-283554.2453*s(69)+290087.3351*s(71)+ ... +37951.06875*s(75)-37951.06875*s(74)+193214.9219*s(45)+ ... +148385.4737*s(72)+3882.546137*s(76)+369220.1805*s(54)+ ... +377727.0134*s(55)+110867.3152*d5*(z1-3.405800000)^5-110867.3152* ... +c0*(z1-3.405800000)^5+164.*s(57)*cos(s(52)-11.72974664*s(51)+ ... +57.32789841*s(50)-149.4314941*s(49)+219.0991957* ... +s(48)-171.3318703*s(47)+55.82442859*c0*(z1-3.405800000)^6)-82.* ... +s(57)*s(68)+478.*s(64)*s(57)+82.*s(57)*s(67)+82.*s(77)* ... +s(67)-478.*s(64)*s(77)+478.*s(65)*s(77)-82.*s(77)*s(68)+1600.* ... +s(36)*sin(s(25))+80.*s(36)*cos(s(25))-478.*s(65)*s(57)-1600.* ... +s(36)*sin(s(13))-80.*s(36)*cos(s(13))+800.*s(24)*sin(s(13))+40.* ... +s(24)*cos(s(13))+800.*s(24)*sin(s(25))+40.*s(24)*cos(s(25))-478.* ... +s(64)*s(24)-478.*s(65)*s(24)+82.*s(24)*s(68)+82.*s(24)* ... +s(67)-1945600.858*s(29)-254535.8689*s(33)-52856.32233*s(21)+ ... +995213.7509*s(31)-995213.7509*s(30)+26040.03754*s(35)+ ... +404018.9169*s(16)-5407.413200*s(22)-404018.9169*s(17)+10560.+ ... +328.*cos(s(52)-11.72974664*s(51)+57.32789841*s(50)-149.4314941* ... +s(49)+219.0991957*s(48)-171.3318703*s(47)+55.82442859*c0* ... +(z1-3.405800000)^6)-154410.3695*c5*(z1-3.405800000)^5+ ... +154410.3695*d0*(z1-3.405800000)^5+743581.3889*a0* ... +(z1-3.405800000)^5-743581.3889*a5*(z1-3.405800000)^5-80.* ... +cos(s(25))-1600.*sin(s(25))+1600.*sin(s(13)))^2*(254535.8689* ... +s(38)-80.*s(39)*cos(s(13))-4586.231873*s(36)*sin(s(13))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+80.*s(39)* ... +cos(s(25))+4586.231873*s(36)*sin(s(25))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-1600.*s(39)*sin(s(13))+ ... +91724.63745*s(36)*cos(s(13))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+995213.7509*s(37)+1600.*s(39)* ... +sin(s(25))-91724.63745*s(36)*cos(s(25))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4+4586.231873*sin(s(13))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-4586.231873* ... +sin(s(25))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-91724.63745*cos(s(13))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+91724.63745*cos(s(25))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4+2293.115936*s(24)*sin(s(25))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+2293.115936* ... +s(24)*sin(s(13))*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-45862.31873*s(24)*cos(s(25))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-45862.31873*s(24)*cos(s(13))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4); + + + % dz_partial_a2 (state two) + dz(2) = 3; + + dz(2) = -112.4773367*sin(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +2249.546734*cos(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + + case 4, + % dz_partial_a3 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = a3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = a0*(-5.658195183+1.954957773*z1)^6; + s(13) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(14) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(15) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(17) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(19) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = c1*(-5.658195183+1.954957773*z1)^5; + s(23) = c0*(-5.658195183+1.954957773*z1)^5; + s(24) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(25) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(26) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(27) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(28) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(29) = a3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(30) = a3_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(31) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(32) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(33) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(34) = a1*(-5.658195183+1.954957773*z1)^5; + s(35) = a0*(-5.658195183+1.954957773*z1)^5; + s(36) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(37) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = 448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(40) = d0*(-5.658195183+1.954957773*z1)^5; + s(41) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = b1*(-5.658195183+1.954957773*z1)^5; + s(43) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(44) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(45) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(46) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(47) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(48) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(49) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(50) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(51) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(52) = d0*(-5.658195183+1.954957773*z1)^6; + s(53) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(55) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(56) = b0*(-5.658195183+1.954957773*z1)^5; + s(57) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(58) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(59) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(60) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(61) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(62) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(63) = b0*(-5.658195183+1.954957773*z1)^6; + s(64) = cos(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6); + s(65) = cos(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6); + s(66) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6); + s(68) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6); + s(69) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = d1*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 4; + + dz(1) = -1000.*z2/(49416.64843*s(46)+1901783.760*s(27)+80.* ... +cos(s(13))-394919.9610*s(14)-10888.*cos(s(6)-11.72974664*s(5)+ ... +57.32789841*s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703* ... +s(1)+55.82442859*d0*(z1-3.405800000)^6)-404018.9169* ... +s(17)-5407.413200*s(22)+26040.03754*s(35)+404018.9169*s(16)+ ... +995213.7509*s(31)-254535.8689*s(33)-1945600.858*s(29)+ ... +206663.7564*s(19)-52856.32233*s(21)+52856.32233*s(20)+ ... +394919.9610*s(15)+1945600.858*s(28)-26040.03754* ... +s(34)-206663.7564*s(18)-3882.546137*s(40)+369220.1805* ... +s(54)-283554.2453*s(69)+5407.413200*s(23)-1901783.760*s(26)+ ... +254535.8689*s(32)-995213.7509*s(30)-193214.9219* ... +s(44)-49416.64843*s(41)+148385.4737*s(72)-290087.3351*s(70)+ ... +5055.520801*s(56)-369220.1805*s(53)+37951.06875* ... +s(75)-37951.06875*s(74)-148385.4737*s(73)+290087.3351*s(71)+ ... +193214.9219*s(45)+283554.2453*s(66)+3882.546137*s(76)+ ... +377727.0134*s(55)-5055.520801*s(42)-377727.0134*s(43)+ ... +110867.3152*d5*(z1-3.405800000)^5-110867.3152*c0* ... +(z1-3.405800000)^5-144361.9723*b5*(z1-3.405800000)^5+144361.9723* ... +b0*(z1-3.405800000)^5+10560.+164.*s(57)*cos(s(52)-11.72974664* ... +s(51)+57.32789841*s(50)-149.4314941*s(49)+219.0991957* ... +s(48)-171.3318703*s(47)+55.82442859*c0*(z1-3.405800000)^6)+1600.* ... +s(36)*sin(s(25))+80.*s(36)*cos(s(25))-478.*s(64)*s(77)-478.* ... +s(65)*s(57)+82.*s(57)*s(67)+478.*s(64)*s(57)+478.*s(65)*s(77)+ ... +82.*s(77)*s(67)-82.*s(77)*s(68)-478.*s(64)*s(24)-478.*s(65)* ... +s(24)+82.*s(24)*s(68)+82.*s(24)*s(67)-82.*s(57)*s(68)-1600.* ... +s(36)*sin(s(13))+800.*s(24)*sin(s(13))-80.*s(36)*cos(s(13))+40.* ... +s(24)*cos(s(13))+40.*s(24)*cos(s(25))+800.*s(24)*sin(s(25))+328.* ... +cos(s(52)-11.72974664*s(51)+57.32789841*s(50)-149.4314941*s(49)+ ... +219.0991957*s(48)-171.3318703*s(47)+55.82442859*c0* ... +(z1-3.405800000)^6)-154410.3695*c5*(z1-3.405800000)^5+ ... +154410.3695*d0*(z1-3.405800000)^5-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5-80.* ... +cos(s(25))-1600.*sin(s(25))+1600.*sin(s(13)))^2*(-995213.7509* ... +s(38)-1945600.858*s(37)-80.*s(39)*cos(s(13))+11954.51953*s(36)* ... +sin(s(13))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +80.*s(39)*cos(s(25))-11954.51953*s(36)*sin(s(25))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-1600.*s(39)* ... +sin(s(13))-239090.3906*s(36)*cos(s(13))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3+1600.*s(39)*sin(s(25))+ ... +239090.3906*s(36)*cos(s(25))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-5977.259765*s(24)*sin(s(25))*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3+119545.1953*s(24)*cos(s(25))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-11954.51953* ... +sin(s(13))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +11954.51953*sin(s(25))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+239090.3906*cos(s(13))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3-239090.3906*cos(s(25))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+119545.1953* ... +s(24)*cos(s(13))*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3-5977.259765*s(24)*sin(s(13))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3); + + + % dz_partial_a3 (state two) + dz(2) = 4; + + dz(2) = 293.1845915*sin(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3_0*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-5863.691830*cos(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3_0*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + + case 5, + % dz_partial_a4 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = a4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = a0*(-5.658195183+1.954957773*z1)^6; + s(13) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(14) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(15) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(17) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(19) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = c1*(-5.658195183+1.954957773*z1)^5; + s(23) = c0*(-5.658195183+1.954957773*z1)^5; + s(24) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(25) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(26) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(27) = a4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(28) = a4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(29) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(30) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(31) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(32) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(33) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(34) = a1*(-5.658195183+1.954957773*z1)^5; + s(35) = a0*(-5.658195183+1.954957773*z1)^5; + s(36) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4_0*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(37) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(38) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(39) = -876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = d0*(-5.658195183+1.954957773*z1)^5; + s(41) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = b1*(-5.658195183+1.954957773*z1)^5; + s(43) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(44) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(45) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(46) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(47) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(48) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(49) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(50) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(51) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(52) = d0*(-5.658195183+1.954957773*z1)^6; + s(53) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(55) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(56) = b0*(-5.658195183+1.954957773*z1)^5; + s(57) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(58) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(59) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(60) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(61) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(62) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(63) = b0*(-5.658195183+1.954957773*z1)^6; + s(64) = cos(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6); + s(65) = cos(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6); + s(66) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6); + s(68) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6); + s(69) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = d1*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 5; + + dz(1) = -1000.*z2/(49416.64843*s(46)+1901783.760*s(27)+80.* ... +cos(s(13))-254535.8689*s(33)+995213.7509*s(31)-52856.32233* ... +s(21)-1945600.858*s(29)+206663.7564*s(19)-144361.9723*b5* ... +(z1-3.405800000)^5+144361.9723*b0*(z1-3.405800000)^5-394919.9610* ... +s(14)-10888.*cos(s(6)-11.72974664*s(5)+57.32789841* ... +s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703*s(1)+ ... +55.82442859*d0*(z1-3.405800000)^6)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+110867.3152* ... +d5*(z1-3.405800000)^5-110867.3152*c0*(z1-3.405800000)^5+328.* ... +cos(s(52)-11.72974664*s(51)+57.32789841*s(50)-149.4314941*s(49)+ ... +219.0991957*s(48)-171.3318703*s(47)+55.82442859*c0* ... +(z1-3.405800000)^6)+40.*s(24)*cos(s(25))+800.*s(24)* ... +sin(s(25))-478.*s(64)*s(24)-478.*s(65)*s(24)+82.*s(24)*s(68)+82.* ... +s(24)*s(67)-82.*s(57)*s(68)+478.*s(64)*s(57)-478.*s(65)*s(57)+ ... +82.*s(57)*s(67)+82.*s(77)*s(67)-478.*s(64)*s(77)+40.*s(24)* ... +cos(s(13))+800.*s(24)*sin(s(13))-1600.*s(36)*sin(s(13))-80.* ... +s(36)*cos(s(13))+404018.9169*s(16)+1945600.858*s(28)+254535.8689* ... +s(32)+52856.32233*s(20)+394919.9610*s(15)-5407.413200* ... +s(22)-404018.9169*s(17)+26040.03754*s(35)-206663.7564*s(18)+ ... +5407.413200*s(23)-26040.03754*s(34)-148385.4737* ... +s(73)-377727.0134*s(43)-369220.1805*s(53)-283554.2453*s(69)+ ... +37951.06875*s(75)-37951.06875*s(74)-3882.546137*s(40)+ ... +148385.4737*s(72)+5055.520801*s(56)+283554.2453* ... +s(66)-290087.3351*s(70)+290087.3351*s(71)+377727.0134*s(55)+ ... +193214.9219*s(45)+3882.546137*s(76)-49416.64843* ... +s(41)-5055.520801*s(42)-193214.9219*s(44)+369220.1805* ... +s(54)-1901783.760*s(26)-995213.7509*s(30)+10560.+164.*s(57)* ... +cos(s(52)-11.72974664*s(51)+57.32789841*s(50)-149.4314941*s(49)+ ... +219.0991957*s(48)-171.3318703*s(47)+55.82442859*c0* ... +(z1-3.405800000)^6)+478.*s(65)*s(77)-82.*s(77)*s(68)+80.*s(36)* ... +cos(s(25))+1600.*s(36)*sin(s(25))-154410.3695*c5* ... +(z1-3.405800000)^5+154410.3695*d0*(z1-3.405800000)^5-80.* ... +cos(s(25))-1600.*sin(s(25))+1600.*sin(s(13)))^2*(1945600.858* ... +s(38)+1901783.760*s(37)-80.*s(39)*cos(s(13))-17527.93566*s(36)* ... +sin(s(13))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +80.*s(39)*cos(s(25))+17527.93566*s(36)*sin(s(25))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2-1600.*s(39)* ... +sin(s(13))+350558.7132*s(36)*cos(s(13))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+1600.*s(39)* ... +sin(s(25))-350558.7132*s(36)*cos(s(25))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+8763.967829*s(24)*sin(s(25))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2-175279.3566* ... +s(24)*cos(s(25))*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+17527.93566*sin(s(13))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-17527.93566*sin(s(25))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-350558.7132*cos(s(13))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+350558.7132* ... +cos(s(25))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-175279.3566*s(24)*cos(s(13))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2+8763.967829*s(24)*sin(s(13))*(z1-3.405800000)^ ... +4*(-5.658195183+1.954957773*z1)^2); + + + % dz_partial_a4 (state two) + dz(2) = 5; + + dz(2) = -429.8726220*sin(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +8597.452440*cos(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^ ... +6)*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + + case 6, + % dz_partial_a5 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = a5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = a0*(-5.658195183+1.954957773*z1)^6; + s(13) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(14) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(15) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(17) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(19) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = c1*(-5.658195183+1.954957773*z1)^5; + s(23) = c0*(-5.658195183+1.954957773*z1)^5; + s(24) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(25) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(26) = a5_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(27) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(28) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(29) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(30) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(31) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(32) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(33) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(34) = a1*(-5.658195183+1.954957773*z1)^5; + s(35) = a0*(-5.658195183+1.954957773*z1)^5; + s(36) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*a5_0*(z1-3.405800000)^ ... +5-334.9465716*a0*(z1-3.405800000)^5; + s(37) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(38) = 856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)+ ... +334.9465716*(z1-3.405800000)^5; + s(39) = d0*(-5.658195183+1.954957773*z1)^5; + s(40) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = b1*(-5.658195183+1.954957773*z1)^5; + s(42) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(43) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(45) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(47) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(48) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(49) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(50) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(51) = d0*(-5.658195183+1.954957773*z1)^6; + s(52) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(53) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(55) = b0*(-5.658195183+1.954957773*z1)^5; + s(56) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(57) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(58) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(59) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(60) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(61) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(62) = b0*(-5.658195183+1.954957773*z1)^6; + s(63) = cos(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6); + s(64) = cos(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6); + s(65) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(66) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6); + s(67) = cos(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6); + s(68) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 6; + + dz(1) = -1000.*z2/(-369220.1805*s(52)+1901783.760*s(27)-144361.9723*b5* ... +(z1-3.405800000)^5+144361.9723*b0*(z1-3.405800000)^5-80.* ... +cos(s(13))-394919.9610*s(14)-10888.*cos(s(6)-11.72974664*s(5)+ ... +57.32789841*s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703* ... +s(1)+55.82442859*d0*(z1-3.405800000)^6)-743581.3889*a5_0* ... +(z1-3.405800000)^5+110867.3152*d5*(z1-3.405800000)^5-110867.3152* ... +c0*(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+80.* ... +s(36)*cos(s(13))+1600.*s(36)*sin(s(13))+800.*s(24)*sin(s(13))+ ... +394919.9610*s(15)-52856.32233*s(21)+404018.9169*s(16)+ ... +995213.7509*s(31)-254535.8689*s(33)-1945600.858*s(29)+ ... +206663.7564*s(19)+254535.8689*s(32)-5407.413200* ... +s(22)-404018.9169*s(17)+26040.03754*s(35)+52856.32233*s(20)+ ... +5407.413200*s(23)-1901783.760*s(26)+1945600.858* ... +s(28)-26040.03754*s(34)-206663.7564*s(18)-49416.64843* ... +s(40)-995213.7509*s(30)+193214.9219*s(44)+290087.3351* ... +s(70)-283554.2453*s(68)+3882.546137*s(75)-377727.0134*s(42)+ ... +37951.06875*s(74)+49416.64843*s(45)-148385.4737* ... +s(72)-37951.06875*s(73)+377727.0134*s(54)+283554.2453* ... +s(65)-3882.546137*s(39)-193214.9219*s(43)+5055.520801* ... +s(55)-5055.520801*s(41)+369220.1805*s(53)-290087.3351*s(69)+ ... +148385.4737*s(71)+10560.+164.*s(56)*cos(s(51)-11.72974664*s(50)+ ... +57.32789841*s(49)-149.4314941*s(48)+219.0991957* ... +s(47)-171.3318703*s(46)+55.82442859*c0*(z1-3.405800000)^6)-1600.* ... +s(36)*sin(s(25))-478.*s(63)*s(76)-80.*s(36)*cos(s(25))-82.*s(76)* ... +s(67)+82.*s(56)*s(66)+478.*s(63)*s(56)-478.*s(64)*s(56)-82.* ... +s(56)*s(67)+82.*s(76)*s(66)+478.*s(64)*s(76)+82.*s(24)*s(67)+40.* ... +s(24)*cos(s(13))+40.*s(24)*cos(s(25))+800.*s(24)*sin(s(25))+82.* ... +s(24)*s(66)-478.*s(63)*s(24)-478.*s(64)*s(24)+328.* ... +cos(s(51)-11.72974664*s(50)+57.32789841*s(49)-149.4314941*s(48)+ ... +219.0991957*s(47)-171.3318703*s(46)+55.82442859*c0* ... +(z1-3.405800000)^6)+154410.3695*d0*(z1-3.405800000)^ ... +5-154410.3695*c5*(z1-3.405800000)^5+80.*cos(s(25))+1600.* ... +sin(s(25))-1600.*sin(s(13)))^2*(-1901783.760*s(37)-743581.3889* ... +(z1-3.405800000)^5-80.*s(38)*cos(s(25))+13706.54962*s(36)* ... +sin(s(25))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+80.* ... +s(38)*cos(s(13))-13706.54962*s(36)*sin(s(13))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-1600.*s(38)*sin(s(25))-274130.9925* ... +s(36)*cos(s(25))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-6853.274812*s(24)*sin(s(13))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+137065.4962*s(24)*cos(s(25))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-6853.274812* ... +s(24)*sin(s(25))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+1600.*s(38)*sin(s(13))+274130.9925*s(36)*cos(s(13))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-13706.54962* ... +sin(s(25))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+ ... +13706.54962*sin(s(13))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+274130.9925*cos(s(25))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-274130.9925*cos(s(13))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+137065.4962* ... +s(24)*cos(s(13))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)); + + + % dz_partial_a5 (state two) + dz(2) = 6; + + dz(2) = 336.1531295*sin(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5_0* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-55.82442859*a0* ... +(z1-3.405800000)^6)*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-6723.062590*cos(-1.*a0*(-5.658195183+1.954957773*z1)^6+ ... +11.72974664*a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-57.32789841*a2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+149.4314941*a3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-219.0991957*a4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+171.3318703*a5_0* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-55.82442859*a0* ... +(z1-3.405800000)^6)*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1); + + case 7, + % dz_partial_b0 (state one) + s(1) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = d0*(-5.658195183+1.954957773*z1)^6; + s(7) = cos(-1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6); + s(8) = -11.72974664*(-5.658195183+1.954957773*z1)^5-334.9465716* ... +(z1-3.405800000)^5; + s(9) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(10) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(11) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(12) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(13) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(14) = b0_0*(-5.658195183+1.954957773*z1)^6; + s(15) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(16) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(17) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(18) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(19) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(20) = c0*(-5.658195183+1.954957773*z1)^6; + s(21) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0_0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(22) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(23) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(24) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(25) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(26) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(27) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(28) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(29) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(30) = b1*(-5.658195183+1.954957773*z1)^5; + s(31) = b0_0*(-5.658195183+1.954957773*z1)^5; + s(32) = -11.72974664*b0_0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716* ... +b0_0*(z1-3.405800000)^5; + s(33) = .5000000000*(-5.658195183+1.954957773*z1)^6+27.91221430* ... +(z1-3.405800000)^6; + s(34) = .5000000000*b0_0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0_0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(35) = -.5000000000*(-5.658195183+1.954957773*z1)^6-27.91221430* ... +(z1-3.405800000)^6; + s(36) = .5000000000*b0_0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0_0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(37) = .5000000000*b0_0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0_0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(38) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(39) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(42) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(43) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(45) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d1*(-5.658195183+1.954957773*z1)^5; + s(47) = d0*(-5.658195183+1.954957773*z1)^5; + s(48) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(49) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = c1*(-5.658195183+1.954957773*z1)^5; + s(58) = c0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(60) = a0*(-5.658195183+1.954957773*z1)^5; + s(61) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(62) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(63) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(64) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(65) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(66) = a0*(-5.658195183+1.954957773*z1)^6; + s(67) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(68) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(69) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(71) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(73) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(75) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(77) = a1*(-5.658195183+1.954957773*z1)^5; + s(78) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 7; + + dz(1) = -1000.*z2/(-404018.9169*s(52)+5407.413200*s(58)+206663.7564* ... +s(54)+995213.7509*s(74)-1901783.760*s(69)-52856.32233*s(56)+ ... +1901783.760*s(70)+404018.9169*s(51)+148385.4737*s(42)+ ... +52856.32233*s(55)-5407.413200*s(57)+37951.06875* ... +s(45)-995213.7509*s(73)+394919.9610*s(50)-206663.7564*s(53)+ ... +26040.03754*s(60)-148385.4737*s(43)+254535.8689* ... +s(75)-254535.8689*s(76)-1945600.858*s(72)+283554.2453* ... +s(38)-394919.9610*s(49)-26040.03754*s(77)-3882.546137* ... +s(47)-290087.3351*s(40)-283554.2453*s(39)-37951.06875*s(44)+ ... +290087.3351*s(41)+1945600.858*s(71)+3882.546137*s(46)+ ... +193214.9219*s(27)+377727.0134*s(24)-5055.520801*s(30)+328.* ... +s(7)-369220.1805*s(22)+369220.1805*s(23)+5055.520801*s(31)+ ... +110867.3152*d5*(z1-3.405800000)^5-110867.3152*c0* ... +(z1-3.405800000)^5-49416.64843*s(29)+49416.64843* ... +s(28)-193214.9219*s(26)+82.*s(32)*cos(s(36))-82.*s(32)* ... +cos(s(37))-478.*cos(s(34))*s(32)+164.*s(32)*s(7)-154410.3695*c5* ... +(z1-3.405800000)^5+154410.3695*d0*(z1-3.405800000)^5-478.* ... +cos(s(34))*s(59)+478.*cos(s(34))*s(48)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+ ... +10560.-10888.*cos(s(20)-11.72974664*s(19)+57.32789841* ... +s(18)-149.4314941*s(17)+219.0991957*s(16)-171.3318703*s(15)+ ... +55.82442859*d0*(z1-3.405800000)^6)+478.*cos(s(21))*s(32)-478.* ... +cos(s(21))*s(48)-478.*cos(s(21))*s(59)+80.*cos(s(67))+1600.* ... +sin(s(67))-80.*cos(s(68))+82.*s(59)*cos(s(37))-82.*s(48)* ... +cos(s(37))-377727.0134*s(25)-144361.9723*b5*(z1-3.405800000)^5+ ... +144361.9723*b0_0*(z1-3.405800000)^5+800.*s(59)*sin(s(67))+40.* ... +s(59)*cos(s(67))+40.*s(59)*cos(s(68))+800.*s(59)*sin(s(68))+80.* ... +s(78)*cos(s(68))-80.*s(78)*cos(s(67))+1600.*s(78)* ... +sin(s(68))-1600.*sin(s(68))+82.*s(48)*cos(s(36))+82.*s(59)* ... +cos(s(36))-1600.*s(78)*sin(s(67)))^2*(82.*s(59)*sin(s(37))*s(35)+ ... +82.*s(59)*sin(s(36))*s(35)+478.*sin(s(21))*s(33)*s(59)+478.* ... +sin(s(34))*s(33)*s(59)+82.*s(48)*sin(s(36))*s(35)-82.*s(48)* ... +sin(s(37))*s(35)+478.*sin(s(21))*s(33)*s(48)-478.*sin(s(34))* ... +s(33)*s(48)+5055.520801*(-5.658195183+1.954957773*z1)^5+ ... +144361.9723*(z1-3.405800000)^5-82.*s(8)*cos(s(37))-82.*s(32)* ... +sin(s(37))*s(35)+82.*s(8)*cos(s(36))+82.*s(32)*sin(s(36))*s(35)+ ... +478.*sin(s(34))*s(33)*s(32)-478.*cos(s(34))*s(8)-478.*sin(s(21))* ... +s(33)*s(32)+478.*cos(s(21))*s(8)+164.*s(8)*s(7)); + + + % dz_partial_b0 (state two) + dz(2) = 7; + + dz(2) = -23.44590000*sin(.5000000000*b0_0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0_0* ... +(z1-3.405800000)^6-.5000000000*d0*(-5.658195183+1.954957773*z1)^ ... +6+5.864873319*d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*d2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*d4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*d5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^ ... +6-1.*z1)*(.5000000000*(-5.658195183+1.954957773*z1)^6+ ... +27.91221430*(z1-3.405800000)^6)-4.022100000*sin(.5000000000*b0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0_0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-1.*z1)*(-.5000000000* ... +(-5.658195183+1.954957773*z1)^6-27.91221430*(z1-3.405800000)^6); + + case 8, + % dz_partial_b1 (state one) + s(1) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(2) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(3) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(4) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(5) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(6) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(7) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(8) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(9) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(10) = c1*(-5.658195183+1.954957773*z1)^5; + s(11) = c0*(-5.658195183+1.954957773*z1)^5; + s(12) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(13) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(14) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(15) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(16) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(17) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(18) = d0*(-5.658195183+1.954957773*z1)^6; + s(19) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(20) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(21) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(22) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(23) = b1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(24) = b0*(-5.658195183+1.954957773*z1)^6; + s(25) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(26) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(27) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(28) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(29) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(30) = c0*(-5.658195183+1.954957773*z1)^6; + s(31) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(32) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(33) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(34) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = d1*(-5.658195183+1.954957773*z1)^5; + s(43) = d0*(-5.658195183+1.954957773*z1)^5; + s(44) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(45) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(46) = cos(-1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6); + s(47) = 11.72974664*(-5.658195183+1.954957773*z1)^5+114.6557968* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(48) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(49) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(51) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(53) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(55) = b1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = b1_0*(-5.658195183+1.954957773*z1)^5; + s(57) = b0*(-5.658195183+1.954957773*z1)^5; + s(58) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1_0* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-114.6557968*b2* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+448.2944824* ... +b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(59) = a0*(-5.658195183+1.954957773*z1)^5; + s(60) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(61) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(62) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(63) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(64) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(65) = a0*(-5.658195183+1.954957773*z1)^6; + s(66) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(67) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(68) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = a1*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 8; + + dz(1) = -1000.*z2/(-193214.9219*s(52)-1901783.760*s(68)-377727.0134* ... +s(51)+290087.3351*s(37)+377727.0134*s(50)-369220.1805*s(48)+ ... +369220.1805*s(49)+328.*s(46)-148385.4737*s(39)-283554.2453* ... +s(35)-37951.06875*s(40)+148385.4737*s(38)+52856.32233*s(8)+ ... +3882.546137*s(42)+394919.9610*s(3)-394919.9610*s(2)-743581.3889* ... +a5*(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^ ... +5-52856.32233*s(9)+5407.413200*s(11)+26040.03754* ... +s(59)-254535.8689*s(75)-1945600.858*s(71)-5055.520801* ... +s(56)-49416.64843*s(55)+193214.9219*s(53)+254535.8689*s(74)+ ... +1901783.760*s(69)+5055.520801*s(57)-3882.546137* ... +s(43)-5407.413200*s(10)+49416.64843*s(54)-26040.03754*s(76)+ ... +1945600.858*s(70)+995213.7509*s(73)-995213.7509*s(72)+82.*s(58)* ... +cos(s(33))+82.*s(44)*cos(s(33))-478.*cos(s(31))*s(44)+478.* ... +cos(s(31))*s(58)-110867.3152*c0*(z1-3.405800000)^5+478.* ... +cos(s(45))*s(44)+80.*s(77)*cos(s(67))+1600.*s(77)*sin(s(67))-82.* ... +s(44)*cos(s(32))-478.*cos(s(31))*s(12)+800.*s(12)*sin(s(66))+82.* ... +s(12)*cos(s(32))+800.*s(12)*sin(s(67))+82.*s(12)*cos(s(33))-478.* ... +cos(s(45))*s(12)+40.*s(12)*cos(s(67))+40.*s(12)*cos(s(66))+164.* ... +s(58)*s(46)+110867.3152*d5*(z1-3.405800000)^5-478.*cos(s(45))* ... +s(58)-82.*s(58)*cos(s(32))-80.*s(77)*cos(s(66))-1600.*s(77)* ... +sin(s(66))+37951.06875*s(41)+206663.7564*s(7)+154410.3695*d0* ... +(z1-3.405800000)^5-10888.*cos(s(30)-11.72974664*s(29)+ ... +57.32789841*s(28)-149.4314941*s(27)+219.0991957* ... +s(26)-171.3318703*s(25)+55.82442859*d0*(z1-3.405800000)^6)+ ... +10560.-154410.3695*c5*(z1-3.405800000)^5+283554.2453* ... +s(34)-404018.9169*s(5)+144361.9723*b0*(z1-3.405800000)^ ... +5-144361.9723*b5*(z1-3.405800000)^5+404018.9169*s(4)-206663.7564* ... +s(6)-290087.3351*s(36)-80.*cos(s(67))-1600.*sin(s(67))+80.* ... +cos(s(66))+1600.*sin(s(66)))^2*(2803.409446*sin(s(45))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(12)-480.9196121*s(44)*sin(s(33))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+480.9196121*s(44)*sin(s(32))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-82.*s(47)* ... +cos(s(32))+480.9196121*s(58)*sin(s(32))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+82.*s(47)*cos(s(33))-480.9196121* ... +s(58)*sin(s(33))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+2803.409446*sin(s(45))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5*s(58)-478.*cos(s(45))*s(47)-2803.409446* ... +sin(s(31))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(58)+478.*cos(s(31))*s(47)+164.*s(47)*s(46)+2803.409446* ... +sin(s(31))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(44)-2803.409446*sin(s(45))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5*s(44)-480.9196121*s(12)*sin(s(33))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-480.9196121* ... +s(12)*sin(s(32))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-5055.520801*(-5.658195183+1.954957773*z1)^5+2803.409446* ... +sin(s(31))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(12)-49416.64843*s(1)); + + + % dz_partial_b1 (state two) + dz(2) = 8; + + dz(2) = -137.5072333*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+z1)*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+23.58910698*sin(-.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*b1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+z1)*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5; + + case 9, + % dz_partial_b2 (state one) + s(1) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(2) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(3) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(4) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(5) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(6) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(7) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(8) = d0*(-5.658195183+1.954957773*z1)^6; + s(9) = cos(-1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6); + s(10) = -114.6557968*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +4-448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(11) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(12) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(13) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(14) = b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(15) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(16) = b0*(-5.658195183+1.954957773*z1)^6; + s(17) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(18) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(19) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(20) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(21) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(22) = c0*(-5.658195183+1.954957773*z1)^6; + s(23) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(24) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(25) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(26) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(27) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(28) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(29) = b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(30) = b2_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(31) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(32) = b1*(-5.658195183+1.954957773*z1)^5; + s(33) = b0*(-5.658195183+1.954957773*z1)^5; + s(34) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824* ... +b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +448.2944824*b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +3+876.3967829*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^2-856.6593514*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^ ... +5-334.9465716*b0*(z1-3.405800000)^5; + s(35) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6; + s(36) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+ ... +.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(37) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+ ... +.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(38) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(39) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(42) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(43) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(45) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d1*(-5.658195183+1.954957773*z1)^5; + s(47) = d0*(-5.658195183+1.954957773*z1)^5; + s(48) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(49) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = c1*(-5.658195183+1.954957773*z1)^5; + s(58) = c0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(60) = a0*(-5.658195183+1.954957773*z1)^5; + s(61) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(62) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(63) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(64) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(65) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(66) = a0*(-5.658195183+1.954957773*z1)^6; + s(67) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(68) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(69) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(71) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(73) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(75) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(77) = a1*(-5.658195183+1.954957773*z1)^5; + s(78) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 9; + + dz(1) = -1000.*z2/(-404018.9169*s(52)+404018.9169*s(51)+394919.9610* ... +s(50)-394919.9610*s(49)-369220.1805*s(24)-1901783.760* ... +s(69)-3882.546137*s(47)+37951.06875*s(45)+3882.546137* ... +s(46)-377727.0134*s(27)+82.*s(34)*cos(s(36))-82.*s(34)* ... +cos(s(37))-478.*cos(s(23))*s(48)-478.*cos(s(23))*s(59)+478.* ... +cos(s(23))*s(34)-49416.64843*s(31)-478.*cos(s(35))*s(34)-1600.* ... +s(78)*sin(s(67))-80.*s(78)*cos(s(67))+82.*s(59)*cos(s(37))-478.* ... +cos(s(35))*s(59)-5055.520801*s(32)+478.*cos(s(35))*s(48)-82.* ... +s(48)*cos(s(37))+164.*s(34)*s(9)+80.*s(78)*cos(s(68))+82.*s(59)* ... +cos(s(36))+82.*s(48)*cos(s(36))+1600.*s(78)*sin(s(68))+40.*s(59)* ... +cos(s(67))+800.*s(59)*sin(s(67))+800.*s(59)*sin(s(68))+40.*s(59)* ... +cos(s(68))+5407.413200*s(58)-148385.4737*s(43)-206663.7564* ... +s(53)-290087.3351*s(40)-52856.32233*s(56)-5407.413200*s(57)+ ... +254535.8689*s(75)+1901783.760*s(70)+52856.32233*s(55)+ ... +26040.03754*s(60)-144361.9723*b5*(z1-3.405800000)^5+144361.9723* ... +b0*(z1-3.405800000)^5-995213.7509*s(73)+49416.64843* ... +s(30)-1945600.858*s(72)+283554.2453*s(38)+995213.7509*s(74)+ ... +206663.7564*s(54)-37951.06875*s(44)+290087.3351* ... +s(41)-254535.8689*s(76)+1945600.858*s(71)-26040.03754*s(77)+ ... +148385.4737*s(42)-283554.2453*s(39)+328.*s(9)+377727.0134* ... +s(26)-110867.3152*c0*(z1-3.405800000)^5+110867.3152*d5* ... +(z1-3.405800000)^5-154410.3695*c5*(z1-3.405800000)^5+154410.3695* ... +d0*(z1-3.405800000)^5+193214.9219*s(29)-193214.9219*s(28)+80.* ... +cos(s(67))+1600.*sin(s(67))+5055.520801*s(33)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5-80.* ... +cos(s(68))+369220.1805*s(25)+10560.-10888.*cos(s(22)-11.72974664* ... +s(21)+57.32789841*s(20)-149.4314941*s(19)+219.0991957* ... +s(18)-171.3318703*s(17)+55.82442859*d0*(z1-3.405800000)^6)-1600.* ... +sin(s(68)))^2*(-2350.443835*s(59)*sin(s(37))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-2350.443835*s(59)*sin(s(36))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+13701.36772* ... +sin(s(23))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4* ... +s(59)+13701.36772*sin(s(35))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(59)-13701.36772*sin(s(35))*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4*s(48)-82.*s(10)*cos(s(37))+ ... +2350.443835*s(34)*sin(s(37))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+13701.36772*sin(s(23))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4*s(48)-2350.443835*s(48)* ... +sin(s(36))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +2350.443835*s(48)*sin(s(37))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+82.*s(10)*cos(s(36))-2350.443835*s(34)* ... +sin(s(36))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +13701.36772*sin(s(35))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(34)-478.*cos(s(35))*s(10)-13701.36772* ... +sin(s(23))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4* ... +s(34)+478.*cos(s(23))*s(10)+164.*s(10)*s(9)+49416.64843*s(2)+ ... +193214.9219*s(1)); + + + % dz_partial_b2 (state two) + dz(2) = 9; + + dz(2) = -672.0520866*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-1.*z1)* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+115.2892701* ... +sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+ ... +.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-1.*z1)*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4; + + case 10, + % dz_partial_b3 (state one) + s(1) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(2) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(3) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(4) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(5) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(6) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(7) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(8) = c0*(-5.658195183+1.954957773*z1)^6; + s(9) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(10) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(11) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(12) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(13) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(14) = d0*(-5.658195183+1.954957773*z1)^6; + s(15) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(16) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(17) = b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(18) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(19) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(20) = b0*(-5.658195183+1.954957773*z1)^6; + s(21) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(22) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(23) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(24) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(25) = b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(26) = b3_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(27) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(28) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(29) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(30) = b1*(-5.658195183+1.954957773*z1)^5; + s(31) = b0*(-5.658195183+1.954957773*z1)^5; + s(32) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(33) = 448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(34) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = d1*(-5.658195183+1.954957773*z1)^5; + s(43) = d0*(-5.658195183+1.954957773*z1)^5; + s(44) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(45) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(46) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(47) = cos(-1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6); + s(48) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(49) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = c1*(-5.658195183+1.954957773*z1)^5; + s(58) = c0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(60) = a0*(-5.658195183+1.954957773*z1)^5; + s(61) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(62) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(63) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(64) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(65) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(66) = a0*(-5.658195183+1.954957773*z1)^6; + s(67) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(68) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(69) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(71) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(73) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(75) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(77) = a1*(-5.658195183+1.954957773*z1)^5; + s(78) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 10; + + dz(1) = -1000.*z2/(-404018.9169*s(52)+404018.9169*s(51)+290087.3351* ... +s(37)+394919.9610*s(50)-394919.9610*s(49)+328.*s(47)+193214.9219* ... +s(27)+5055.520801*s(31)-283554.2453*s(35)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5-148385.4737* ... +s(39)+37951.06875*s(41)+3882.546137*s(42)-37951.06875* ... +s(40)-3882.546137*s(43)-26040.03754*s(77)+254535.8689* ... +s(75)-254535.8689*s(76)+1901783.760*s(70)+1945600.858*s(71)+ ... +995213.7509*s(74)-1945600.858*s(72)-52856.32233*s(56)+ ... +148385.4737*s(38)-5407.413200*s(57)-995213.7509*s(73)+ ... +26040.03754*s(60)+206663.7564*s(54)+5407.413200*s(58)+ ... +52856.32233*s(55)-1901783.760*s(69)-5055.520801*s(30)+ ... +283554.2453*s(34)+377727.0134*s(24)-206663.7564* ... +s(53)-369220.1805*s(22)+154410.3695*d0*(z1-3.405800000)^ ... +5-154410.3695*c5*(z1-3.405800000)^5+82.*s(59)*cos(s(21))-82.* ... +s(32)*cos(s(21))-82.*s(44)*cos(s(21))+82.*s(44)*cos(s(48))+478.* ... +cos(s(45))*s(44)-478.*cos(s(46))*s(44)+800.*s(59)*sin(s(68))+40.* ... +s(59)*cos(s(67))+40.*s(59)*cos(s(68))+82.*s(59)*cos(s(48))+800.* ... +s(59)*sin(s(67))-478.*cos(s(46))*s(59)-478.*cos(s(45))*s(59)+82.* ... +s(32)*cos(s(48))+164.*s(32)*s(47)+478.*cos(s(46))*s(32)-478.* ... +cos(s(45))*s(32)-144361.9723*b5*(z1-3.405800000)^5+110867.3152* ... +d5*(z1-3.405800000)^5-193214.9219*s(26)-1600.*s(78)* ... +sin(s(67))-80.*s(78)*cos(s(67))+80.*s(78)*cos(s(68))+1600.*s(78)* ... +sin(s(68))-110867.3152*c0*(z1-3.405800000)^5+144361.9723*b0* ... +(z1-3.405800000)^5+10560.-49416.64843*s(29)+49416.64843* ... +s(28)-10888.*cos(s(8)-11.72974664*s(7)+57.32789841* ... +s(6)-149.4314941*s(5)+219.0991957*s(4)-171.3318703*s(3)+ ... +55.82442859*d0*(z1-3.405800000)^6)+369220.1805*s(23)-290087.3351* ... +s(36)+80.*cos(s(67))+1600.*sin(s(67))-80.*cos(s(68))-377727.0134* ... +s(25)-1600.*sin(s(68)))^2*(-6126.691259*s(59)*sin(s(21))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-6126.691259* ... +s(59)*sin(s(48))*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+35714.12710*sin(s(46))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(59)+35714.12710*sin(s(45))*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3*s(59)+6126.691259*s(44)* ... +sin(s(21))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-6126.691259*s(44)*sin(s(48))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+82.*s(33)*cos(s(48))-6126.691259*s(32)* ... +sin(s(48))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +35714.12710*sin(s(45))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(32)-478.*cos(s(45))*s(33)-35714.12710* ... +sin(s(46))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3* ... +s(32)+478.*cos(s(46))*s(33)+164.*s(33)*s(47)+35714.12710* ... +sin(s(46))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3* ... +s(44)-35714.12710*sin(s(45))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(44)-82.*s(33)*cos(s(21))+6126.691259*s(32)* ... +sin(s(21))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-193214.9219*s(2)-377727.0134*s(1)); + + + % dz_partial_b3 (state two) + dz(2) = 10; + + dz(2) = -1751.777934*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3_0*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+z1)*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3+300.5142063*sin(-.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*b3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+z1)*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3; + + case 11, + % dz_partial_b4 (state one) + s(1) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(2) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(3) = -876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(4) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(5) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(6) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(7) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(8) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(9) = c0*(-5.658195183+1.954957773*z1)^6; + s(10) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(11) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(12) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(13) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(14) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(15) = d0*(-5.658195183+1.954957773*z1)^6; + s(16) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(17) = b4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(18) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(19) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(20) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(21) = b0*(-5.658195183+1.954957773*z1)^6; + s(22) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(23) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(24) = b4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(25) = b4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(26) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(27) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(28) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(29) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(30) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(31) = b1*(-5.658195183+1.954957773*z1)^5; + s(32) = b0*(-5.658195183+1.954957773*z1)^5; + s(33) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4_0*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(34) = cos(-1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6); + s(35) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(36) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(37) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(38) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(39) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(42) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(43) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(45) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d1*(-5.658195183+1.954957773*z1)^5; + s(47) = d0*(-5.658195183+1.954957773*z1)^5; + s(48) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(49) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = c1*(-5.658195183+1.954957773*z1)^5; + s(58) = c0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(60) = a0*(-5.658195183+1.954957773*z1)^5; + s(61) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(62) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(63) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(64) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(65) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(66) = a0*(-5.658195183+1.954957773*z1)^6; + s(67) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(68) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(69) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(71) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(73) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(75) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(77) = a1*(-5.658195183+1.954957773*z1)^5; + s(78) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 11; + + dz(1) = -1000.*z2/(-404018.9169*s(52)-148385.4737*s(43)+52856.32233* ... +s(55)+5407.413200*s(58)+148385.4737*s(42)-283554.2453*s(39)+ ... +404018.9169*s(51)+206663.7564*s(54)-37951.06875*s(44)+ ... +290087.3351*s(41)-52856.32233*s(56)+26040.03754*s(60)+ ... +394919.9610*s(50)-1901783.760*s(69)-1945600.858*s(72)+ ... +1945600.858*s(71)-5407.413200*s(57)-206663.7564* ... +s(53)-995213.7509*s(73)+254535.8689*s(75)+995213.7509* ... +s(74)-394919.9610*s(49)-254535.8689*s(76)-26040.03754*s(77)+ ... +1901783.760*s(70)-3882.546137*s(47)+283554.2453*s(38)+ ... +37951.06875*s(45)-290087.3351*s(40)+3882.546137* ... +s(46)-193214.9219*s(27)-5055.520801*s(31)+5055.520801*s(32)+ ... +369220.1805*s(24)-49416.64843*s(30)+328.*s(34)-478.*cos(s(35))* ... +s(59)-478.*cos(s(35))*s(48)+110867.3152*d5*(z1-3.405800000)^ ... +5-110867.3152*c0*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-154410.3695*c5*(z1-3.405800000)^5-82.*s(33)* ... +cos(s(37))+478.*cos(s(35))*s(33)-377727.0134*s(26)-743581.3889* ... +a5*(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+ ... +49416.64843*s(29)+193214.9219*s(28)+82.*s(33)*cos(s(36))+164.* ... +s(33)*s(34)+10560.-10888.*cos(s(9)-11.72974664*s(8)+57.32789841* ... +s(7)-149.4314941*s(6)+219.0991957*s(5)-171.3318703*s(4)+ ... +55.82442859*d0*(z1-3.405800000)^6)-369220.1805*s(23)+80.* ... +cos(s(67))+1600.*sin(s(67))-478.*cos(s(22))*s(33)+478.* ... +cos(s(22))*s(48)-478.*cos(s(22))*s(59)-144361.9723*b5* ... +(z1-3.405800000)^5+144361.9723*b0*(z1-3.405800000)^5-80.* ... +cos(s(68))+82.*s(59)*cos(s(37))-82.*s(48)*cos(s(37))+377727.0134* ... +s(25)+40.*s(59)*cos(s(68))+800.*s(59)*sin(s(68))+800.*s(59)* ... +sin(s(67))+40.*s(59)*cos(s(67))-1600.*sin(s(68))-80.*s(78)* ... +cos(s(67))-1600.*s(78)*sin(s(67))+82.*s(48)*cos(s(36))+82.*s(59)* ... +cos(s(36))+80.*s(78)*cos(s(68))+1600.*s(78)*sin(s(68)))^2* ... +(-8983.067025*s(59)*sin(s(37))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-8983.067025*s(59)*sin(s(36))*(z1-3.405800000)^ ... +4*(-5.658195183+1.954957773*z1)^2+52364.70778*sin(s(35))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2*s(59)+ ... +52364.70778*sin(s(22))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(59)-8983.067025*s(48)*sin(s(36))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+8983.067025* ... +s(48)*sin(s(37))*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+52364.70778*sin(s(35))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(48)-52364.70778*sin(s(22))*(z1-3.405800000)^ ... +4*(-5.658195183+1.954957773*z1)^2*s(48)-82.*s(3)*cos(s(37))+ ... +8983.067025*s(33)*sin(s(37))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2+82.*s(3)*cos(s(36))-8983.067025*s(33)* ... +sin(s(36))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-52364.70778*sin(s(35))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(33)+478.*cos(s(35))*s(3)+164.*s(3)*s(34)+ ... +52364.70778*sin(s(22))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(33)-478.*cos(s(22))*s(3)+377727.0134*s(2)+ ... +369220.1805*s(1)); + + + % dz_partial_b4 (state two) + dz(2) = 11; + + dz(2) = -2568.488917*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-1.*z1)* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+440.6194376* ... +sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4_0*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+ ... +.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-1.*z1)*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2; + + case 12, + % dz_partial_b5 (state one) + s(1) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(2) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(3) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(4) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(5) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(6) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(7) = c0*(-5.658195183+1.954957773*z1)^6; + s(8) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(9) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(10) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(11) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(12) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(13) = d0*(-5.658195183+1.954957773*z1)^6; + s(14) = b5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(15) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(16) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(17) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(18) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(19) = b0*(-5.658195183+1.954957773*z1)^6; + s(20) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(21) = b5_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(22) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(23) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(24) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(25) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(26) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(27) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(28) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(29) = b1*(-5.658195183+1.954957773*z1)^5; + s(30) = b0*(-5.658195183+1.954957773*z1)^5; + s(31) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*b5_0*(z1-3.405800000)^ ... +5-334.9465716*b0*(z1-3.405800000)^5; + s(32) = 856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)+ ... +334.9465716*(z1-3.405800000)^5; + s(33) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(34) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = d1*(-5.658195183+1.954957773*z1)^5; + s(43) = d0*(-5.658195183+1.954957773*z1)^5; + s(44) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(45) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(46) = cos(-1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6); + s(47) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(48) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(49) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(51) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(53) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(55) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(-5.658195183+1.954957773*z1)^5; + s(57) = c0*(-5.658195183+1.954957773*z1)^5; + s(58) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(59) = a0*(-5.658195183+1.954957773*z1)^5; + s(60) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(61) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(62) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(63) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(64) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(65) = a0*(-5.658195183+1.954957773*z1)^6; + s(66) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(67) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(68) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(70) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(72) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(74) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(76) = a1*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 12; + + dz(1) = -1000.*z2/(-206663.7564*s(52)-1901783.760*s(68)-404018.9169* ... +s(51)+290087.3351*s(37)-144361.9723*b5_0*(z1-3.405800000)^5+ ... +404018.9169*s(50)-394919.9610*s(48)+144361.9723*b0* ... +(z1-3.405800000)^5+394919.9610*s(49)+328.*s(46)-369220.1805* ... +s(21)+49416.64843*s(27)-154410.3695*c5*(z1-3.405800000)^5+ ... +154410.3695*d0*(z1-3.405800000)^5-283554.2453*s(35)+110867.3152* ... +d5*(z1-3.405800000)^5-110867.3152*c0*(z1-3.405800000)^5+ ... +37951.06875*s(41)+148385.4737*s(38)+5055.520801*s(30)+ ... +283554.2453*s(34)-377727.0134*s(24)+3882.546137* ... +s(42)-148385.4737*s(39)+1901783.760*s(69)-52856.32233*s(55)+ ... +206663.7564*s(53)+5407.413200*s(57)+995213.7509* ... +s(73)-1945600.858*s(71)+1945600.858*s(70)-254535.8689*s(75)+ ... +26040.03754*s(59)+254535.8689*s(74)-995213.7509* ... +s(72)-5407.413200*s(56)-26040.03754*s(76)-37951.06875* ... +s(40)-3882.546137*s(43)+52856.32233*s(54)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+369220.1805* ... +s(22)+10560.+800.*s(58)*sin(s(66))+80.*s(77)*cos(s(67))+1600.* ... +s(77)*sin(s(67))-478.*cos(s(45))*s(44)-478.*cos(s(47))*s(58)+40.* ... +s(58)*cos(s(66))+40.*s(58)*cos(s(67))+800.*s(58)*sin(s(67))+478.* ... +cos(s(47))*s(44)-478.*cos(s(45))*s(58)+82.*s(58)*cos(s(33))+82.* ... +s(44)*cos(s(33))-478.*cos(s(47))*s(31)+478.*cos(s(45))*s(31)-82.* ... +s(44)*cos(s(20))+82.*s(58)*cos(s(20))-82.*s(31)*cos(s(20))-80.* ... +s(77)*cos(s(66))-1600.*s(77)*sin(s(66))+164.*s(31)*s(46)+82.* ... +s(31)*cos(s(33))+193214.9219*s(26)-5055.520801*s(29)-49416.64843* ... +s(28)-10888.*cos(s(7)-11.72974664*s(6)+57.32789841* ... +s(5)-149.4314941*s(4)+219.0991957*s(3)-171.3318703*s(2)+ ... +55.82442859*d0*(z1-3.405800000)^6)+377727.0134*s(23)-290087.3351* ... +s(36)-80.*cos(s(67))-1600.*sin(s(67))-193214.9219*s(25)+80.* ... +cos(s(66))+1600.*sin(s(66)))^2*(-7024.606682*s(58)*sin(s(20))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-7024.606682* ... +s(58)*sin(s(33))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+40948.31700*sin(s(45))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)*s(58)+40948.31700*sin(s(47))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)*s(58)-40948.31700*sin(s(47))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)*s(44)+82.*s(32)* ... +cos(s(33))-7024.606682*s(31)*sin(s(33))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+40948.31700*sin(s(47))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)*s(31)-478.* ... +cos(s(47))*s(32)-40948.31700*sin(s(45))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)*s(31)+478.*cos(s(45))*s(32)+164.* ... +s(32)*s(46)+7024.606682*s(44)*sin(s(20))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+40948.31700*sin(s(45))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)* ... +s(44)-7024.606682*s(44)*sin(s(33))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-82.*s(32)*cos(s(20))+7024.606682* ... +s(31)*sin(s(20))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-369220.1805*s(1)-144361.9723*(z1-3.405800000)^5); + + + % dz_partial_b5 (state two) + dz(2) = 12; + + dz(2) = -2008.514949*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5_0* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-27.91221430*b0* ... +(z1-3.405800000)^6+.5000000000*d0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*d2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*d4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*d5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*c0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+ ... +344.5569577*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5_0* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-27.91221430*b0* ... +(z1-3.405800000)^6-.5000000000*d0*(-5.658195183+1.954957773*z1)^ ... +6+5.864873319*d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*d2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*d4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*d5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + + case 13, + % dz_partial_c0 (state one) + s(1) = -27.91221430*(z1-3.405800000)^6-.5000000000*(-5.658195183+ ... +1.954957773*z1)^6; + s(2) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(3) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(4) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(5) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(6) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(7) = c0_0*(-5.658195183+1.954957773*z1)^6; + s(8) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(9) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(10) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(11) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(12) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(13) = d0*(-5.658195183+1.954957773*z1)^6; + s(14) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(15) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(16) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(17) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(18) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(19) = b0*(-5.658195183+1.954957773*z1)^6; + s(20) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0_0*(z1-3.405800000)^6-2.*z1+.5000000000*c0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(21) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(22) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(23) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(24) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(25) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(26) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(27) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(28) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(29) = b1*(-5.658195183+1.954957773*z1)^5; + s(30) = b0*(-5.658195183+1.954957773*z1)^5; + s(31) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(32) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0_0*(z1-3.405800000)^6+2.*z1+.5000000000*c0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(33) = d0*(-5.658195183+1.954957773*z1)^6-11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+55.82442859*c0_0*(z1-3.405800000)^6; + s(34) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = d1*(-5.658195183+1.954957773*z1)^5; + s(43) = d0*(-5.658195183+1.954957773*z1)^5; + s(44) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716* ... +c0_0*(z1-3.405800000)^5; + s(45) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(46) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(47) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(48) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(49) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(50) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(51) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(52) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(53) = c1*(-5.658195183+1.954957773*z1)^5; + s(54) = c0_0*(-5.658195183+1.954957773*z1)^5; + s(55) = -11.72974664*c0_0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(56) = -27.91221430*(z1-3.405800000)^6+.5000000000*(-5.658195183+ ... +1.954957773*z1)^6; + s(57) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0_0*(z1-3.405800000)^6+2.*z1+.5000000000*c0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(58) = .5000000000*c0_0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0_0*(z1-3.405800000)^6; + s(59) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(60) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(61) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(62) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(63) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(64) = a0*(-5.658195183+1.954957773*z1)^6; + s(65) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000* ... +c0_0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(66) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000* ... +c0_0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(67) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = a1*(-5.658195183+1.954957773*z1)^5; + s(76) = a0*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + + dz(1) = 13; + + dz(1) = -1000.*z2/(-1901783.760*s(67)-52856.32233*s(52)+1901783.760* ... +s(68)+52856.32233*s(51)+290087.3351*s(37)+206663.7564* ... +s(50)-404018.9169*s(48)-206663.7564*s(49)+404018.9169* ... +s(47)-394919.9610*s(45)+394919.9610*s(46)-369220.1805*s(21)+ ... +49416.64843*s(27)-283554.2453*s(35)-5407.413200*s(53)+ ... +5407.413200*s(54)-82.*s(44)*cos(s(20))-82.*s(31)*cos(s(20))+80.* ... +s(77)*cos(s(66))-377727.0134*s(24)+254535.8689*s(73)-995213.7509* ... +s(71)-254535.8689*s(74)+995213.7509*s(72)+1945600.858* ... +s(69)-1945600.858*s(70)-26040.03754*s(75)+26040.03754* ... +s(76)-3882.546137*s(43)+800.*s(55)*sin(s(65))-478.*cos(s(58))* ... +s(55)+82.*s(55)*cos(s(57))+82.*s(44)*cos(s(57))+478.*cos(s(32))* ... +s(44)-478.*cos(s(32))*s(55)+148385.4737*s(38)+3882.546137* ... +s(42)-37951.06875*s(40)-478.*cos(s(32))*s(31)+40.*s(55)* ... +cos(s(66))+82.*s(31)*cos(s(57))+82.*s(55)*cos(s(20))-478.* ... +cos(s(58))*s(44)+40.*s(55)*cos(s(65))+478.*cos(s(58))*s(31)+800.* ... +s(55)*sin(s(66))+164.*s(31)*cos(s(33))-80.*s(77)* ... +cos(s(65))-1600.*s(77)*sin(s(65))+1600.*s(77)*sin(s(66))+ ... +110867.3152*d5*(z1-3.405800000)^5-110867.3152*c0_0* ... +(z1-3.405800000)^5+5055.520801*s(30)+283554.2453*s(34)+ ... +369220.1805*s(22)-154410.3695*c5*(z1-3.405800000)^5+154410.3695* ... +d0*(z1-3.405800000)^5+193214.9219*s(26)-5055.520801* ... +s(29)-49416.64843*s(28)-10888.*cos(s(7)-11.72974664*s(6)+ ... +57.32789841*s(5)-149.4314941*s(4)+219.0991957*s(3)-171.3318703* ... +s(2)+55.82442859*d0*(z1-3.405800000)^6)+37951.06875*s(41)+ ... +377727.0134*s(23)-148385.4737*s(39)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+328.* ... +cos(s(33))-290087.3351*s(36)+10560.+80.*cos(s(65))+1600.* ... +sin(s(65))-193214.9219*s(25)-144361.9723*b5*(z1-3.405800000)^5+ ... +144361.9723*b0*(z1-3.405800000)^5-80.*cos(s(66))-1600.* ... +sin(s(66)))^2*(-40.*s(77)*sin(s(65))*(-5.658195183+1.954957773* ... +z1)^6+478.*sin(s(58))*s(56)*s(44)-82.*s(44)*sin(s(57))*s(56)-82.* ... +s(55)*sin(s(57))*s(56)+478.*sin(s(58))*s(56)*s(55)-478.* ... +sin(s(58))*s(56)*s(31)-82.*s(31)*sin(s(57))*s(56)-40.*s(77)* ... +sin(s(66))*(-5.658195183+1.954957773*z1)^6-478.*sin(s(32))*s(1)* ... +s(55)+478.*sin(s(32))*s(1)*s(44)-478.*sin(s(32))*s(1)*s(31)-82.* ... +s(44)*sin(s(20))*s(1)+82.*s(55)*sin(s(20))*s(1)-82.*s(31)* ... +sin(s(20))*s(1)-20.*s(55)*sin(s(66))*(-5.658195183+1.954957773* ... +z1)^6+800.*s(77)*cos(s(66))*(-5.658195183+1.954957773*z1)^6+400.* ... +s(55)*cos(s(66))*(-5.658195183+1.954957773*z1)^6-400.*s(55)* ... +cos(s(65))*(-5.658195183+1.954957773*z1)^6+20.*s(55)*sin(s(65))* ... +(-5.658195183+1.954957773*z1)^6-9155.206290*s(31)*sin(s(33))* ... +(z1-3.405800000)^6+40.*sin(s(65))*(-5.658195183+1.954957773*z1)^ ... +6+40.*sin(s(66))*(-5.658195183+1.954957773*z1)^6+5606.818893* ... +cos(s(58))*(-5.658195183+1.954957773*z1)^5+160104.4612* ... +cos(s(58))*(z1-3.405800000)^5-27465.61887*(z1-3.405800000)^5* ... +cos(s(57))-961.8392243*(-5.658195183+1.954957773*z1)^5* ... +cos(s(57))-9383.797310*(-5.658195183+1.954957773*z1)^5* ... +sin(s(66))-800.*cos(s(66))*(-5.658195183+1.954957773*z1)^ ... +6-469.1898655*(-5.658195183+1.954957773*z1)^5*cos(s(66))-800.* ... +cos(s(65))*(-5.658195183+1.954957773*z1)^6-9383.797310* ... +(-5.658195183+1.954957773*z1)^5*sin(s(65))-469.1898655* ... +(-5.658195183+1.954957773*z1)^5*cos(s(65))+10888.* ... +sin(s(7)-11.72974664*s(6)+57.32789841*s(5)-149.4314941*s(4)+ ... +219.0991957*s(3)-171.3318703*s(2)+55.82442859*d0* ... +(z1-3.405800000)^6)*(-5.658195183+1.954957773*z1)^6-160104.4612* ... +cos(s(32))*(z1-3.405800000)^5+5606.818893*cos(s(32))* ... +(-5.658195183+1.954957773*z1)^5-18310.41258*sin(s(33))* ... +(z1-3.405800000)^6-961.8392243*(-5.658195183+1.954957773*z1)^5* ... +cos(s(20))+27465.61887*(z1-3.405800000)^5*cos(s(20))+800.*s(77)* ... +cos(s(65))*(-5.658195183+1.954957773*z1)^6-110867.3152* ... +(z1-3.405800000)^5+5407.413200*(-5.658195183+1.954957773*z1)^5); + + + % dz_partial_c0 (state two) + dz(2) = 13; + + dz(2) = -76.46895000*sin(.5000000000*c0_0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6+z1)*(-5.658195183+1.954957773*z1)^6+66.75705000* ... +sin(.5000000000*c0_0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-1.*z1)*(-5.658195183+1.954957773*z1)^6-654.4269852* ... +sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0_0*(z1-3.405800000)^6+z1)*(z1-3.405800000)^6+ ... +112.2657171*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0_0*(z1-3.405800000)^6-1.*z1)*(z1-3.405800000)^6; + + case 14, + % dz_partial_c1 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = d0*(-5.658195183+1.954957773*z1)^6; + s(13) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(14) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(15) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(16) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(17) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(18) = b0*(-5.658195183+1.954957773*z1)^6; + s(19) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6; + s(20) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = 11.72974664*(-5.658195183+1.954957773*z1)^5+114.6557968* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(23) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(24) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(25) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(26) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(27) = a0*(-5.658195183+1.954957773*z1)^6; + s(28) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(29) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6; + s(30) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(31) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(32) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(33) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(34) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(35) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(36) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(37) = c1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(38) = c1_0*(-5.658195183+1.954957773*z1)^5; + s(39) = c0*(-5.658195183+1.954957773*z1)^5; + s(40) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1_0* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-114.6557968*c2* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+448.2944824* ... +c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(41) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(42) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(43) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6; + s(44) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(45) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(46) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(47) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(48) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(49) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(50) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(51) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(52) = a1*(-5.658195183+1.954957773*z1)^5; + s(53) = a0*(-5.658195183+1.954957773*z1)^5; + s(54) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(55) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(56) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(57) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(58) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(59) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(60) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(61) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(62) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(63) = d1*(-5.658195183+1.954957773*z1)^5; + s(64) = d0*(-5.658195183+1.954957773*z1)^5; + s(65) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(66) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = b1*(-5.658195183+1.954957773*z1)^5; + s(75) = b0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + + dz(1) = 14; + + dz(1) = -1000.*z2/(369220.1805*s(67)-26040.03754*s(52)-369220.1805*s(66)+ ... +377727.0134*s(68)-254535.8689*s(51)-52856.32233* ... +s(37)-1901783.760*s(44)+110867.3152*d5*(z1-3.405800000)^5+ ... +254535.8689*s(50)-995213.7509*s(48)-290087.3351*s(57)+ ... +995213.7509*s(49)-743581.3889*a5*(z1-3.405800000)^5-154410.3695* ... +c5*(z1-3.405800000)^5+5055.520801*s(75)-110867.3152*c0* ... +(z1-3.405800000)^5-5055.520801*s(74)-49416.64843*s(73)+ ... +283554.2453*s(55)-193214.9219*s(70)+193214.9219*s(71)+ ... +49416.64843*s(72)-377727.0134*s(69)-148385.4737*s(60)+ ... +3882.546137*s(63)+37951.06875*s(62)-3882.546137* ... +s(64)-37951.06875*s(61)+148385.4737*s(59)-283554.2453* ... +s(56)-1945600.858*s(47)+290087.3351*s(58)+743581.3889*a0* ... +(z1-3.405800000)^5+1901783.760*s(45)+328.*cos(s(12)-11.72974664* ... +s(11)+57.32789841*s(10)-149.4314941*s(9)+219.0991957* ... +s(8)-171.3318703*s(7)+55.82442859*c0*(z1-3.405800000)^6)+ ... +154410.3695*d0*(z1-3.405800000)^5+1945600.858*s(46)+26040.03754* ... +s(53)+394919.9610*s(31)+206663.7564*s(35)-5407.413200*s(38)+ ... +404018.9169*s(32)+1600.*sin(s(28))+80.*cos(s(28))+800.*s(40)* ... +sin(s(28))+10560.-80.*s(54)*cos(s(28))-1600.*s(54)* ... +sin(s(28))-10888.*cos(s(6)-11.72974664*s(5)+57.32789841* ... +s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703*s(1)+ ... +55.82442859*d0*(z1-3.405800000)^6)+40.*s(40)* ... +cos(s(28))-394919.9610*s(30)-206663.7564*s(34)+82.*s(65)* ... +cos(s(19))+82.*s(76)*cos(s(19))+82.*s(40)*cos(s(19))-478.* ... +cos(s(41))*s(65)+478.*cos(s(41))*s(76)+5407.413200* ... +s(39)-144361.9723*b5*(z1-3.405800000)^5+144361.9723*b0* ... +(z1-3.405800000)^5+800.*s(40)*sin(s(42))+40.*s(40)* ... +cos(s(42))-478.*cos(s(41))*s(40)+52856.32233*s(36)-1600.* ... +sin(s(42))-478.*cos(s(43))*s(40)-404018.9169*s(33)+164.*s(76)* ... +cos(s(12)-11.72974664*s(11)+57.32789841*s(10)-149.4314941*s(9)+ ... +219.0991957*s(8)-171.3318703*s(7)+55.82442859*c0* ... +(z1-3.405800000)^6)-80.*cos(s(42))+478.*cos(s(43))*s(65)-478.* ... +cos(s(43))*s(76)-82.*s(65)*cos(s(29))+82.*s(40)*cos(s(29))-82.* ... +s(76)*cos(s(29))+80.*s(54)*cos(s(42))+1600.*s(54)*sin(s(42)))^2* ... +(4691.898655*s(40)*cos(s(28))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+469.1898655*s(54)*sin(s(28))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-9383.797310*s(54)*cos(s(28))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-234.5949327* ... +s(40)*sin(s(28))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-480.9196121*s(40)*sin(s(29))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+2803.409446*sin(s(41))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5*s(40)+480.9196121*s(65)* ... +sin(s(29))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+ ... +480.9196121*s(76)*sin(s(29))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+9383.797310*cos(s(42))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-469.1898655*sin(s(42))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-127713.4814* ... +sin(s(6)-11.72974664*s(5)+57.32789841*s(4)-149.4314941*s(3)+ ... +219.0991957*s(2)-171.3318703*s(1)+55.82442859*d0* ... +(z1-3.405800000)^6)*(z1-3.405800000)*(-5.658195183+1.954957773* ... +z1)^5+9383.797310*cos(s(28))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+2803.409446*sin(s(43))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5*s(40)+2803.409446*sin(s(41))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5*s(65)+ ... +469.1898655*s(54)*sin(s(42))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5-4691.898655*s(40)*cos(s(42))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-480.9196121*s(65)*sin(s(19))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-2803.409446* ... +sin(s(41))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(76)-480.9196121*s(76)*sin(s(19))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+234.5949327*s(40)*sin(s(42))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-480.9196121* ... +s(40)*sin(s(19))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-2803.409446*sin(s(43))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5*s(65)-9383.797310*s(54)*cos(s(42))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+2803.409446* ... +sin(s(43))*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(76)-469.1898655*sin(s(28))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+82.*s(21)*cos(s(19))-52856.32233*s(20)-478.* ... +cos(s(43))*s(21)+82.*s(21)*cos(s(29))+40.*s(21)*cos(s(42))+800.* ... +s(21)*sin(s(42))-478.*cos(s(41))*s(21)+40.*s(21)*cos(s(28))+800.* ... +s(21)*sin(s(28))-5407.413200*(-5.658195183+1.954957773*z1)^5); + + + % dz_partial_c1 (state two) + dz(2) = 14; + + dz(2) = -896.9614091*sin(-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*c1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+ ... +783.0432828*sin(-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*c1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + + case 15, + % dz_partial_c2 (state one) + s(1) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(2) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(3) = -114.6557968*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +4-448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(4) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(5) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(6) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(7) = c2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(8) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(9) = c0*(-5.658195183+1.954957773*z1)^6; + s(10) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(11) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(12) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(13) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(14) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(15) = d0*(-5.658195183+1.954957773*z1)^6; + s(16) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(17) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(18) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(19) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(20) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(21) = b0*(-5.658195183+1.954957773*z1)^6; + s(22) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(23) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(24) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(25) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(26) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(27) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(28) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(29) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(30) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(31) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(32) = d1*(-5.658195183+1.954957773*z1)^5; + s(33) = d0*(-5.658195183+1.954957773*z1)^5; + s(34) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(35) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-2.*z1+.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6; + s(36) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(37) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(38) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(39) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(40) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(41) = a0*(-5.658195183+1.954957773*z1)^6; + s(42) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(43) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(44) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(45) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(46) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(47) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(48) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(49) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(50) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(51) = a1*(-5.658195183+1.954957773*z1)^5; + s(52) = a0*(-5.658195183+1.954957773*z1)^5; + s(53) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(54) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(55) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(56) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(57) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(58) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(59) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(60) = c2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(61) = c2_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(62) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(63) = c1*(-5.658195183+1.954957773*z1)^5; + s(64) = c0*(-5.658195183+1.954957773*z1)^5; + s(65) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824* ... +c2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +448.2944824*c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +3+876.3967829*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^2-856.6593514*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^ ... +5-334.9465716*d0*(z1-3.405800000)^5; + s(66) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = b1*(-5.658195183+1.954957773*z1)^5; + s(75) = b0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(77) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + + dz(1) = 15; + + dz(1) = -1000.*z2/(369220.1805*s(67)+26040.03754*s(52)-369220.1805*s(66)+ ... +377727.0134*s(68)-26040.03754*s(51)-254535.8689*s(50)+ ... +995213.7509*s(48)-1901783.760*s(43)+404018.9169*s(57)+ ... +254535.8689*s(49)-995213.7509*s(47)-404018.9169*s(58)+ ... +1945600.858*s(45)-1945600.858*s(46)-154410.3695*c5* ... +(z1-3.405800000)^5+154410.3695*d0*(z1-3.405800000)^5+290087.3351* ... +s(27)+110867.3152*d5*(z1-3.405800000)^5-110867.3152*c0* ... +(z1-3.405800000)^5+37951.06875*s(31)+3882.546137* ... +s(32)-144361.9723*b5*(z1-3.405800000)^5+144361.9723*b0* ... +(z1-3.405800000)^5-206663.7564*s(59)+49416.64843*s(72)+ ... +193214.9219*s(71)-193214.9219*s(70)-49416.64843* ... +s(73)-5407.413200*s(63)+5407.413200*s(64)+394919.9610*s(56)+ ... +1901783.760*s(44)+206663.7564*s(60)+52856.32233* ... +s(61)-394919.9610*s(55)-5055.520801*s(74)+5055.520801* ... +s(75)-52856.32233*s(62)-377727.0134*s(69)+40.*s(65)*cos(s(77))+ ... +82.*s(76)*cos(s(54))+82.*s(65)*cos(s(54))-478.*cos(s(35))*s(65)+ ... +478.*cos(s(35))*s(76)+800.*s(65)*sin(s(77))+800.*s(65)* ... +sin(s(42))-1600.*s(53)*sin(s(42))+1600.*s(53)*sin(s(77))+80.* ... +s(53)*cos(s(77))+40.*s(65)*cos(s(42))-82.*s(34)*cos(s(23))+82.* ... +s(34)*cos(s(54))+478.*cos(s(22))*s(34)+82.*s(65)*cos(s(23))-82.* ... +s(76)*cos(s(23))-478.*cos(s(22))*s(65)-478.*cos(s(22))*s(76)-80.* ... +s(53)*cos(s(42))-478.*cos(s(35))*s(34)+283554.2453* ... +s(24)-37951.06875*s(30)+10560.+328.*cos(s(15)-11.72974664*s(14)+ ... +57.32789841*s(13)-149.4314941*s(12)+219.0991957* ... +s(11)-171.3318703*s(10)+55.82442859*c0*(z1-3.405800000)^6)+164.* ... +s(76)*cos(s(15)-11.72974664*s(14)+57.32789841*s(13)-149.4314941* ... +s(12)+219.0991957*s(11)-171.3318703*s(10)+55.82442859*c0* ... +(z1-3.405800000)^6)+743581.3889*a0*(z1-3.405800000)^ ... +5-743581.3889*a5*(z1-3.405800000)^5-290087.3351* ... +s(26)-148385.4737*s(29)+148385.4737*s(28)-10888.* ... +cos(s(9)-11.72974664*s(8)+57.32789841*s(7)-149.4314941*s(6)+ ... +219.0991957*s(5)-171.3318703*s(4)+55.82442859*d0* ... +(z1-3.405800000)^6)+1600.*sin(s(42))-3882.546137*s(33)-1600.* ... +sin(s(77))-80.*cos(s(77))-283554.2453*s(25)+80.*cos(s(42)))^2* ... +(13701.36772*sin(s(35))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(65)-2350.443835*s(76)*sin(s(54))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-2350.443835* ... +s(65)*sin(s(54))*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+13701.36772*sin(s(22))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(65)+13701.36772*sin(s(22))*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4*s(76)+45862.31873*s(53)* ... +cos(s(42))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-2350.443835*s(65)*sin(s(23))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+2350.443835*s(76)*sin(s(23))*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-13701.36772*sin(s(22))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4* ... +s(34)-1146.557968*s(65)*sin(s(77))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-2293.115936*s(53)*sin(s(77))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+13701.36772* ... +sin(s(35))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4* ... +s(34)+2350.443835*s(34)*sin(s(23))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4+45862.31873*s(53)*cos(s(77))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+52856.32233* ... +s(2)-13701.36772*sin(s(35))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(76)+1146.557968*s(65)*sin(s(42))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-2293.115936* ... +s(53)*sin(s(42))*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-22931.15936*s(65)*cos(s(42))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-2350.443835*s(34)*sin(s(54))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-45862.31873* ... +cos(s(42))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +22931.15936*s(65)*cos(s(77))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+2293.115936*sin(s(42))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4+2293.115936*sin(s(77))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+624186.1579* ... +sin(s(9)-11.72974664*s(8)+57.32789841*s(7)-149.4314941*s(6)+ ... +219.0991957*s(5)-171.3318703*s(4)+55.82442859*d0* ... +(z1-3.405800000)^6)*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-45862.31873*cos(s(77))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4+82.*s(3)*cos(s(23))-478.*cos(s(22))*s(3)+40.* ... +s(3)*cos(s(42))-478.*cos(s(35))*s(3)+40.*s(3)*cos(s(77))+800.* ... +s(3)*sin(s(77))+800.*s(3)*sin(s(42))+82.*s(3)*cos(s(54))+ ... +206663.7564*s(1)); + + + % dz_partial_c2 (state two) + dz(2) = 15; + + dz(2) = -4383.804197*sin(.5000000000*c0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*c2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +3827.041380*sin(.5000000000*c0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*c2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + + case 16, + % dz_partial_c3 (state one) + s(1) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(3) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(4) = c3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(5) = c3_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(6) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(7) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(8) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(9) = c1*(-5.658195183+1.954957773*z1)^5; + s(10) = c0*(-5.658195183+1.954957773*z1)^5; + s(11) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(12) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(13) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(14) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(15) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(16) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(17) = d0*(-5.658195183+1.954957773*z1)^6; + s(18) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(19) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(20) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(21) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(22) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(23) = b0*(-5.658195183+1.954957773*z1)^6; + s(24) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(25) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(26) = c3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(27) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(28) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(29) = c0*(-5.658195183+1.954957773*z1)^6; + s(30) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(31) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(32) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(33) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(34) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(35) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(36) = a0*(-5.658195183+1.954957773*z1)^6; + s(37) = a0*(-5.658195183+1.954957773*z1)^6-11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6-1.*z1; + s(38) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(39) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(42) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(43) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(45) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = a1*(-5.658195183+1.954957773*z1)^5; + s(47) = a0*(-5.658195183+1.954957773*z1)^5; + s(48) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(49) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(50) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(51) = 448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(53) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(54) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(55) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(56) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(57) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(58) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(59) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(60) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(61) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(62) = d1*(-5.658195183+1.954957773*z1)^5; + s(63) = d0*(-5.658195183+1.954957773*z1)^5; + s(64) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(65) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(66) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = b1*(-5.658195183+1.954957773*z1)^5; + s(75) = b0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(77) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + + dz(1) = 16; + + dz(1) = -1000.*z2/(369220.1805*s(67)+283554.2453*s(54)-369220.1805*s(66)+ ... +377727.0134*s(68)-995213.7509*s(42)+37951.06875*s(61)+ ... +3882.546137*s(62)-37951.06875*s(60)-1945600.858* ... +s(41)-290087.3351*s(56)-148385.4737*s(59)+995213.7509* ... +s(43)-3882.546137*s(63)-283554.2453*s(55)+290087.3351*s(57)+ ... +5055.520801*s(75)-5055.520801*s(74)+193214.9219* ... +s(71)-377727.0134*s(69)-49416.64843*s(73)-193214.9219*s(70)+ ... +49416.64843*s(72)+26040.03754*s(47)+148385.4737* ... +s(58)-254535.8689*s(45)-26040.03754*s(46)+404018.9169*s(3)+ ... +394919.9610*s(2)+52856.32233*s(7)+478.*cos(s(30))*s(76)-478.* ... +cos(s(30))*s(64)+164.*s(76)*cos(s(17)-11.72974664*s(16)+ ... +57.32789841*s(15)-149.4314941*s(14)+219.0991957* ... +s(13)-171.3318703*s(12)+55.82442859*c0*(z1-3.405800000)^6)+ ... +5407.413200*s(10)-394919.9610*s(1)-154410.3695*c5* ... +(z1-3.405800000)^5+154410.3695*d0*(z1-3.405800000)^5+328.* ... +cos(s(17)-11.72974664*s(16)+57.32789841*s(15)-149.4314941*s(14)+ ... +219.0991957*s(13)-171.3318703*s(12)+55.82442859*c0* ... +(z1-3.405800000)^6)-10888.*cos(s(29)-11.72974664*s(28)+ ... +57.32789841*s(27)-149.4314941*s(26)+219.0991957* ... +s(25)-171.3318703*s(24)+55.82442859*d0*(z1-3.405800000)^6)+ ... +10560.-206663.7564*s(5)-5407.413200*s(9)-52856.32233*s(8)+ ... +1945600.858*s(40)-404018.9169*s(4)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+82.*s(64)* ... +cos(s(77))-1901783.760*s(38)+1901783.760*s(39)+254535.8689* ... +s(44)-1600.*s(48)*sin(s(37))+80.*s(48)*cos(s(37))+478.* ... +cos(s(65))*s(64)+82.*s(11)*cos(s(77))+82.*s(11)*cos(s(53))-478.* ... +cos(s(65))*s(11)+40.*s(11)*cos(s(37))-800.*s(11)*sin(s(37))+40.* ... +s(11)*cos(s(52))+800.*s(11)*sin(s(52))-478.*cos(s(30))*s(11)+ ... +206663.7564*s(6)+110867.3152*d5*(z1-3.405800000)^5-110867.3152* ... +c0*(z1-3.405800000)^5-80.*s(48)*cos(s(52))-1600.*s(48)* ... +sin(s(52))+1600.*sin(s(37))-80.*cos(s(37))+144361.9723*b0* ... +(z1-3.405800000)^5-144361.9723*b5*(z1-3.405800000)^5+82.*s(76)* ... +cos(s(77))+80.*cos(s(52))-82.*s(76)*cos(s(53))-82.*s(64)* ... +cos(s(53))+1600.*sin(s(52))-478.*cos(s(65))*s(76))^2* ... +(-206663.7564*s(50)-404018.9169*s(49)-6126.691259*s(76)* ... +sin(s(77))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-119545.1953*s(48)*cos(s(37))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-5977.259765*s(48)*sin(s(37))*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3+35714.12710*sin(s(30))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3* ... +s(64)-35714.12710*sin(s(30))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(76)+6126.691259*s(76)*sin(s(53))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+6126.691259* ... +s(64)*sin(s(53))*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3-2988.629883*s(11)*sin(s(37))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3+59772.59765*s(11)*cos(s(52))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-59772.59765* ... +s(11)*cos(s(37))*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3-2988.629883*s(11)*sin(s(52))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3+35714.12710*sin(s(65))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3* ... +s(11)-6126.691259*s(11)*sin(s(77))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3-6126.691259*s(11)*sin(s(53))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-1627010.108* ... +sin(s(29)-11.72974664*s(28)+57.32789841*s(27)-149.4314941*s(26)+ ... +219.0991957*s(25)-171.3318703*s(24)+55.82442859*d0* ... +(z1-3.405800000)^6)*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+5977.259765*sin(s(37))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+119545.1953*cos(s(37))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3-5977.259765*sin(s(52))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+119545.1953* ... +cos(s(52))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-478.*cos(s(30))*s(51)+35714.12710*sin(s(30))*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3*s(11)-6126.691259*s(64)* ... +sin(s(77))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-35714.12710*sin(s(65))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(64)+35714.12710*sin(s(65))*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3*s(76)+5977.259765*s(48)* ... +sin(s(52))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-119545.1953*s(48)*cos(s(52))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+40.*s(51)*cos(s(37))-800.*s(51)*sin(s(37))+82.* ... +s(51)*cos(s(77))+82.*s(51)*cos(s(53))-478.*cos(s(65))*s(51)+800.* ... +s(51)*sin(s(52))+40.*s(51)*cos(s(52))); + + + % dz_partial_c3 (state two) + dz(2) = 16; + + dz(2) = -11426.86945*sin(-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*c3_0*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +9975.605725*sin(-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*c3_0*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + + case 17, + % dz_partial_c4 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = a0*(-5.658195183+1.954957773*z1)^6; + s(13) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(14) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(15) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(16) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(17) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(18) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(19) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(20) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(21) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(22) = a1*(-5.658195183+1.954957773*z1)^5; + s(23) = a0*(-5.658195183+1.954957773*z1)^5; + s(24) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(25) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(26) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(27) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(28) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(29) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(30) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(31) = d0*(-5.658195183+1.954957773*z1)^6; + s(32) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(33) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(34) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(35) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(36) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(37) = b0*(-5.658195183+1.954957773*z1)^6; + s(38) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(39) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = -876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(42) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(43) = c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(44) = c4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(45) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(46) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(47) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(48) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(49) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(50) = c1*(-5.658195183+1.954957773*z1)^5; + s(51) = c0*(-5.658195183+1.954957773*z1)^5; + s(52) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4_0*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(53) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(54) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(55) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(56) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(57) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(58) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(59) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(60) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(61) = b1*(-5.658195183+1.954957773*z1)^5; + s(62) = b0*(-5.658195183+1.954957773*z1)^5; + s(63) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(64) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(65) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+ ... +.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(66) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(67) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = d1*(-5.658195183+1.954957773*z1)^5; + s(76) = d0*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + + dz(1) = 17; + + dz(1) = -1000.*z2/(283554.2453*s(67)+369220.1805*s(54)-369220.1805* ... +s(53)-283554.2453*s(68)+5407.413200*s(51)-394919.9610*s(42)+ ... +5055.520801*s(62)-5407.413200*s(50)+52856.32233*s(48)+ ... +49416.64843*s(59)+377727.0134*s(55)+404018.9169*s(44)+ ... +394919.9610*s(43)-193214.9219*s(57)-377727.0134* ... +s(56)-52856.32233*s(49)-5055.520801*s(61)-49416.64843* ... +s(60)-3882.546137*s(76)+3882.546137*s(75)+37951.06875*s(74)+ ... +148385.4737*s(71)-148385.4737*s(72)-37951.06875* ... +s(73)-290087.3351*s(69)+290087.3351*s(70)+206663.7564*s(47)+ ... +193214.9219*s(58)-404018.9169*s(45)-206663.7564* ... +s(46)-254535.8689*s(21)+80.*cos(s(13))-1901783.760*s(14)+80.* ... +s(24)*cos(s(25))+1600.*s(24)*sin(s(25))-1945600.858*s(17)-10888.* ... +cos(s(6)-11.72974664*s(5)+57.32789841*s(4)-149.4314941*s(3)+ ... +219.0991957*s(2)-171.3318703*s(1)+55.82442859*d0* ... +(z1-3.405800000)^6)+995213.7509*s(19)-26040.03754*s(22)+ ... +110867.3152*d5*(z1-3.405800000)^5-110867.3152*c0* ... +(z1-3.405800000)^5+1945600.858*s(16)+328.*cos(s(31)-11.72974664* ... +s(30)+57.32789841*s(29)-149.4314941*s(28)+219.0991957* ... +s(27)-171.3318703*s(26)+55.82442859*c0*(z1-3.405800000)^6)+ ... +10560.+164.*s(63)*cos(s(31)-11.72974664*s(30)+57.32789841* ... +s(29)-149.4314941*s(28)+219.0991957*s(27)-171.3318703*s(26)+ ... +55.82442859*c0*(z1-3.405800000)^6)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+254535.8689* ... +s(20)+26040.03754*s(23)+478.*cos(s(64))*s(77)+82.*s(63)* ... +cos(s(38))+82.*s(77)*cos(s(38))+82.*s(52)*cos(s(38))+1901783.760* ... +s(15)-154410.3695*c5*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-995213.7509*s(18)-478.*cos(s(65))*s(52)+82.* ... +s(52)*cos(s(66))-478.*cos(s(64))*s(52)+40.*s(52)*cos(s(25))+800.* ... +s(52)*sin(s(25))-144361.9723*b5*(z1-3.405800000)^5+144361.9723* ... +b0*(z1-3.405800000)^5-478.*cos(s(64))*s(63)-80.*s(24)* ... +cos(s(13))-82.*s(77)*cos(s(66))+478.*cos(s(65))*s(63)-82.*s(63)* ... +cos(s(66))-1600.*s(24)*sin(s(13))+800.*s(52)*sin(s(13))+40.* ... +s(52)*cos(s(13))-80.*cos(s(25))-1600.*sin(s(25))+1600.* ... +sin(s(13))-478.*cos(s(65))*s(77))^2*(394919.9610*s(39)+ ... +404018.9169*s(40)+8763.967829*sin(s(25))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-8983.067025*s(63)*sin(s(38))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+175279.3566* ... +s(24)*cos(s(25))*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-8763.967829*s(24)*sin(s(25))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+52364.70778*sin(s(65))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2*s(52)+ ... +52364.70778*sin(s(64))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(52)+4381.983915*s(52)*sin(s(13))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+175279.3566* ... +s(24)*cos(s(13))*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-87639.67829*s(52)*cos(s(13))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-4381.983915*s(52)*sin(s(25))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+87639.67829* ... +s(52)*cos(s(25))*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-52364.70778*sin(s(64))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(77)-52364.70778*sin(s(65))*(z1-3.405800000)^ ... +4*(-5.658195183+1.954957773*z1)^2*s(63)-8983.067025*s(77)* ... +sin(s(38))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-8983.067025*s(52)*sin(s(38))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2+8983.067025*s(77)*sin(s(66))*(z1-3.405800000)^ ... +4*(-5.658195183+1.954957773*z1)^2+8983.067025*s(63)*sin(s(66))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+52364.70778* ... +sin(s(64))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2* ... +s(63)-8763.967829*s(24)*sin(s(13))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+52364.70778*sin(s(65))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2* ... +s(77)-8983.067025*s(52)*sin(s(66))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+2385552.043*sin(s(6)-11.72974664* ... +s(5)+57.32789841*s(4)-149.4314941*s(3)+219.0991957* ... +s(2)-171.3318703*s(1)+55.82442859*d0*(z1-3.405800000)^6)* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2-175279.3566* ... +cos(s(25))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +8763.967829*sin(s(13))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-175279.3566*cos(s(13))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-478.*cos(s(65))*s(41)+82.*s(41)* ... +cos(s(66))-478.*cos(s(64))*s(41)+82.*s(41)*cos(s(38))+800.*s(41)* ... +sin(s(25))+40.*s(41)*cos(s(25))+40.*s(41)*cos(s(13))+800.*s(41)* ... +sin(s(13))); + + + % dz_partial_c4 (state two) + dz(2) = 17; + + dz(2) = -16754.28544*sin(.5000000000*c0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*c4_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +14626.41596*sin(.5000000000*c0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*c4_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*d0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + + case 18, + % dz_partial_c5 (state one) + s(1) = c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = d0*(-5.658195183+1.954957773*z1)^6; + s(13) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(14) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(15) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(16) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(17) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(18) = b0*(-5.658195183+1.954957773*z1)^6; + s(19) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(20) = c5_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(21) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(22) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(23) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(24) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(25) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(26) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(27) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(28) = c1*(-5.658195183+1.954957773*z1)^5; + s(29) = c0*(-5.658195183+1.954957773*z1)^5; + s(30) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*c5_0*(z1-3.405800000)^ ... +5-334.9465716*d0*(z1-3.405800000)^5; + s(31) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(32) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(33) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(34) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(35) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(36) = a0*(-5.658195183+1.954957773*z1)^6; + s(37) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + s(38) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(39) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(42) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(43) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(45) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = a1*(-5.658195183+1.954957773*z1)^5; + s(47) = a0*(-5.658195183+1.954957773*z1)^5; + s(48) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(49) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(50) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(51) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(52) = 856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)+ ... +334.9465716*(z1-3.405800000)^5; + s(53) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(54) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(55) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(56) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(57) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(58) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(59) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(60) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(61) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(62) = d1*(-5.658195183+1.954957773*z1)^5; + s(63) = d0*(-5.658195183+1.954957773*z1)^5; + s(64) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(65) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(66) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = b1*(-5.658195183+1.954957773*z1)^5; + s(75) = b0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + + dz(1) = 18; + + dz(1) = -1000.*z2/(369220.1805*s(67)+283554.2453*s(54)-369220.1805*s(66)+ ... +377727.0134*s(68)+37951.06875*s(61)-995213.7509* ... +s(42)-290087.3351*s(56)-1945600.858*s(41)+995213.7509* ... +s(43)-148385.4737*s(59)-1901783.760*s(38)-283554.2453*s(55)+ ... +290087.3351*s(57)-37951.06875*s(60)+3882.546137*s(62)+ ... +5055.520801*s(75)-5055.520801*s(74)-49416.64843*s(73)+ ... +193214.9219*s(71)-193214.9219*s(70)-377727.0134*s(69)+ ... +49416.64843*s(72)+26040.03754*s(47)+148385.4737* ... +s(58)-254535.8689*s(45)+328.*cos(s(12)-11.72974664*s(11)+ ... +57.32789841*s(10)-149.4314941*s(9)+219.0991957*s(8)-171.3318703* ... +s(7)+55.82442859*c0*(z1-3.405800000)^6)-3882.546137* ... +s(63)-26040.03754*s(46)+394919.9610*s(21)-52856.32233*s(27)+ ... +743581.3889*a0*(z1-3.405800000)^5-743581.3889*a5* ... +(z1-3.405800000)^5-206663.7564*s(24)+40.*s(30)*cos(s(53))-478.* ... +cos(s(65))*s(30)+800.*s(30)*sin(s(53))+82.*s(30)*cos(s(50))+40.* ... +s(30)*cos(s(37))+800.*s(30)*sin(s(37))-478.*cos(s(49))* ... +s(30)-10888.*cos(s(6)-11.72974664*s(5)+57.32789841* ... +s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703*s(1)+ ... +55.82442859*d0*(z1-3.405800000)^6)+110867.3152*d5* ... +(z1-3.405800000)^5-110867.3152*c0*(z1-3.405800000)^5+404018.9169* ... +s(22)+154410.3695*d0*(z1-3.405800000)^5-154410.3695*c5_0* ... +(z1-3.405800000)^5+144361.9723*b0*(z1-3.405800000)^5-144361.9723* ... +b5*(z1-3.405800000)^5-82.*s(64)*cos(s(19))-82.*s(76)*cos(s(19))+ ... +82.*s(30)*cos(s(19))-80.*cos(s(53))+52856.32233*s(26)+10560.+ ... +164.*s(76)*cos(s(12)-11.72974664*s(11)+57.32789841* ... +s(10)-149.4314941*s(9)+219.0991957*s(8)-171.3318703*s(7)+ ... +55.82442859*c0*(z1-3.405800000)^6)+254535.8689*s(44)+5407.413200* ... +s(29)-5407.413200*s(28)-394919.9610*s(20)+1901783.760*s(39)+ ... +1945600.858*s(40)-404018.9169*s(23)-80.*s(48)*cos(s(37))-1600.* ... +s(48)*sin(s(37))+82.*s(76)*cos(s(50))+82.*s(64)*cos(s(50))-478.* ... +cos(s(49))*s(76)+1600.*s(48)*sin(s(53))+80.*s(48)*cos(s(53))+ ... +478.*cos(s(49))*s(64)+1600.*sin(s(37))+206663.7564*s(25)+80.* ... +cos(s(37))+478.*cos(s(65))*s(76)-1600.*sin(s(53))-478.* ... +cos(s(65))*s(64))^2*(-394919.9610*s(51)+68532.74812*s(30)* ... +cos(s(37))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-7024.606682*s(30)*sin(s(50))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+40948.31700*sin(s(65))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)*s(30)+ ... +3426.637406*s(30)*sin(s(53))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-68532.74812*s(30)*cos(s(53))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-7024.606682*s(64)*sin(s(50))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+6853.274812* ... +s(48)*sin(s(37))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-137065.4962*s(48)*cos(s(37))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-40948.31700*sin(s(49))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)* ... +s(64)-137065.4962*s(48)*cos(s(53))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+6853.274812*s(48)*sin(s(53))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-40948.31700* ... +sin(s(65))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)* ... +s(76)-7024.606682*s(30)*sin(s(19))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+7024.606682*s(76)*sin(s(19))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+7024.606682* ... +s(64)*sin(s(19))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+40948.31700*sin(s(65))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)*s(64)+40948.31700*sin(s(49))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)*s(76)-7024.606682*s(76)*sin(s(50))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+40948.31700* ... +sin(s(49))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)* ... +s(30)-3426.637406*s(30)*sin(s(37))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+82.*s(52)*cos(s(19))-154410.3695* ... +(z1-3.405800000)^5-6853.274812*sin(s(37))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+137065.4962*cos(s(37))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+137065.4962* ... +cos(s(53))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-6853.274812*sin(s(53))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-1865461.404*sin(s(6)-11.72974664*s(5)+ ... +57.32789841*s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703* ... +s(1)+55.82442859*d0*(z1-3.405800000)^6)*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+800.*s(52)*sin(s(37))+40.*s(52)* ... +cos(s(37))+82.*s(52)*cos(s(50))-478.*cos(s(49))*s(52)-478.* ... +cos(s(65))*s(52)+40.*s(52)*cos(s(53))+800.*s(52)*sin(s(53))); + + + % dz_partial_c5 (state two) + dz(2) = 18; + + dz(2) = -13101.56822*sin(-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*c5_0* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-27.91221430*d0* ... +(z1-3.405800000)^6-1.*z1)*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+11437.61023*sin(-.5000000000*c0*(-5.658195183+ ... +1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4+74.71574707*c3*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3-109.5495979*c4* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+85.66593514* ... +c5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1)*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1); + + case 19, + % dz_partial_d0 (state one) + s(1) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(2) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(3) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(4) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(5) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(6) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(7) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(8) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(9) = b1*(-5.658195183+1.954957773*z1)^5; + s(10) = b0*(-5.658195183+1.954957773*z1)^5; + s(11) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(12) = -.5000000000*(-5.658195183+1.954957773*z1)^6-27.91221430* ... +(z1-3.405800000)^6; + s(13) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(14) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(15) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(16) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(17) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(18) = c0*(-5.658195183+1.954957773*z1)^6; + s(19) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(20) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(21) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(22) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(23) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(24) = d0_0*(-5.658195183+1.954957773*z1)^6; + s(25) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(26) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(27) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(28) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(29) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(30) = b0*(-5.658195183+1.954957773*z1)^6; + s(31) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000* ... +d0_0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0_0*(z1-3.405800000)^6; + s(32) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0_0*(z1-3.405800000)^6; + s(33) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(34) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(36) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(38) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(40) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = d1*(-5.658195183+1.954957773*z1)^5; + s(42) = d0_0*(-5.658195183+1.954957773*z1)^5; + s(43) = -11.72974664*d0_0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(44) = d0_0*(-5.658195183+1.954957773*z1)^6-11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+55.82442859*c0*(z1-3.405800000)^6; + s(45) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(46) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(47) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(48) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(49) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(50) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(51) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(52) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(53) = c1*(-5.658195183+1.954957773*z1)^5; + s(54) = c0*(-5.658195183+1.954957773*z1)^5; + s(55) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716* ... +d0_0*(z1-3.405800000)^5; + s(56) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(57) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(58) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(59) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(60) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(61) = a0*(-5.658195183+1.954957773*z1)^6; + s(62) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0_0*(z1-3.405800000)^6+z1; + s(63) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000* ... +d0_0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+ ... +.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0_0*(z1-3.405800000)^6; + s(64) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0_0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0_0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(65) = -.5000000000*(-5.658195183+1.954957773*z1)^6+27.91221430* ... +(z1-3.405800000)^6; + s(66) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(-5.658195183+1.954957773*z1)^5; + s(75) = a0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(77) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0_0*(z1-3.405800000)^6+z1; + + dz(1) = 19; + + dz(1) = -1000.*z2/(1901783.760*s(67)-52856.32233*s(52)+5407.413200* ... +s(54)-1901783.760*s(66)-5407.413200*s(53)+1945600.858*s(68)+ ... +52856.32233*s(51)+148385.4737*s(37)-3882.546137*s(42)+ ... +154410.3695*d0_0*(z1-3.405800000)^5+3882.546137*s(41)+ ... +206663.7564*s(50)-404018.9169*s(48)-148385.4737* ... +s(38)-154410.3695*c5*(z1-3.405800000)^5-206663.7564*s(49)+ ... +144361.9723*b0*(z1-3.405800000)^5+110867.3152*d5* ... +(z1-3.405800000)^5-144361.9723*b5*(z1-3.405800000)^5+26040.03754* ... +s(75)-1945600.858*s(69)-995213.7509*s(70)+995213.7509*s(71)+ ... +254535.8689*s(72)-26040.03754*s(74)-254535.8689*s(73)+ ... +404018.9169*s(47)-394919.9610*s(45)+394919.9610*s(46)+ ... +377727.0134*s(3)-290087.3351*s(35)-37951.06875*s(39)-10888.* ... +cos(s(18)-11.72974664*s(17)+57.32789841*s(16)-149.4314941*s(15)+ ... +219.0991957*s(14)-171.3318703*s(13)+55.82442859*d0_0* ... +(z1-3.405800000)^6)+10560.+369220.1805*s(2)+478.*cos(s(64))* ... +s(11)+82.*s(11)*cos(s(63))+164.*s(11)*cos(s(44))-82.*s(11)* ... +cos(s(32))-478.*cos(s(31))*s(11)+49416.64843*s(7)-110867.3152*c0* ... +(z1-3.405800000)^5-283554.2453*s(34)+5055.520801* ... +s(10)-369220.1805*s(1)+82.*s(55)*cos(s(63))-193214.9219*s(5)+82.* ... +s(55)*cos(s(32))-5055.520801*s(9)-82.*s(43)*cos(s(32))+328.* ... +cos(s(44))+37951.06875*s(40)-49416.64843*s(8)-478.*cos(s(31))* ... +s(55)-377727.0134*s(4)+478.*cos(s(31))*s(43)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5+193214.9219* ... +s(6)+290087.3351*s(36)+283554.2453*s(33)-478.*cos(s(64))*s(55)+ ... +800.*s(55)*sin(s(62))+40.*s(55)*cos(s(62))+40.*s(55)*cos(s(77))+ ... +800.*s(55)*sin(s(77))+1600.*sin(s(77))+80.*cos(s(77))-1600.* ... +sin(s(62))+80.*s(76)*cos(s(62))+1600.*s(76)*sin(s(62))-80.* ... +cos(s(62))-80.*s(76)*cos(s(77))-1600.*s(76)*sin(s(77))-478.* ... +cos(s(64))*s(43)+82.*s(43)*cos(s(63)))^2*(-267957.2573* ... +(z1-3.405800000)^5*sin(s(77))-2232.977144*s(76)*sin(s(77))* ... +(z1-3.405800000)^6+44659.54288*s(76)*cos(s(77))*(z1-3.405800000)^ ... +6-2232.977144*s(76)*sin(s(62))*(z1-3.405800000)^6+44659.54288* ... +s(76)*cos(s(62))*(z1-3.405800000)^6-22329.77144*s(55)*cos(s(77))* ... +(z1-3.405800000)^6+22329.77144*s(55)*cos(s(62))*(z1-3.405800000)^ ... +6-1116.488572*s(55)*sin(s(62))*(z1-3.405800000)^6+5606.818893* ... +cos(s(64))*(-5.658195183+1.954957773*z1)^5+1116.488572*s(55)* ... +sin(s(77))*(z1-3.405800000)^6-164.*s(11)*sin(s(44))* ... +(-5.658195183+1.954957773*z1)^6+607816.3785* ... +sin(s(18)-11.72974664*s(17)+57.32789841*s(16)-149.4314941*s(15)+ ... +219.0991957*s(14)-171.3318703*s(13)+55.82442859*d0_0* ... +(z1-3.405800000)^6)*(z1-3.405800000)^6-5606.818893*cos(s(31))* ... +(-5.658195183+1.954957773*z1)^5+961.8392243*(-5.658195183+ ... +1.954957773*z1)^5*cos(s(32))-13397.86286*(z1-3.405800000)^5* ... +cos(s(62))-267957.2573*(z1-3.405800000)^5*sin(s(62))-328.* ... +sin(s(44))*(-5.658195183+1.954957773*z1)^6-961.8392243* ... +(-5.658195183+1.954957773*z1)^5*cos(s(63))-27465.61887* ... +(z1-3.405800000)^5*cos(s(63))-27465.61887*(z1-3.405800000)^5* ... +cos(s(32))+160104.4612*cos(s(31))*(z1-3.405800000)^5+2232.977144* ... +sin(s(77))*(z1-3.405800000)^6+160104.4612*cos(s(64))* ... +(z1-3.405800000)^5+2232.977144*sin(s(62))*(z1-3.405800000)^ ... +6-44659.54288*cos(s(62))*(z1-3.405800000)^6-13397.86286* ... +(z1-3.405800000)^5*cos(s(77))-44659.54288*cos(s(77))* ... +(z1-3.405800000)^6-478.*sin(s(64))*s(65)*s(11)-82.*s(11)* ... +sin(s(32))*s(12)-478.*sin(s(31))*s(12)*s(11)-82.*s(11)* ... +sin(s(63))*s(65)+478.*sin(s(31))*s(12)*s(43)-82.*s(43)* ... +sin(s(32))*s(12)-478.*sin(s(31))*s(12)*s(55)+82.*s(55)* ... +sin(s(32))*s(12)-3882.546137*(-5.658195183+1.954957773*z1)^5+ ... +154410.3695*(z1-3.405800000)^5+478.*sin(s(64))*s(65)*s(55)-82.* ... +s(55)*sin(s(63))*s(65)-82.*s(43)*sin(s(63))*s(65)+478.* ... +sin(s(64))*s(65)*s(43)); + + + % dz_partial_d0 (state two) + dz(2) = 19; + + dz(2) = -4268.835439*sin(.5000000000*c0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*c2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*c4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*d0_0* ... +(z1-3.405800000)^6+z1)*(z1-3.405800000)^6+3726.674171* ... +sin(.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*c3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*c5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*d0_0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)^6-11.72295000*sin(-.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0_0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+z1)*(-5.658195183+ ... +1.954957773*z1)^6+2.011050000*sin(.5000000000*b0*(-5.658195183+ ... +1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-74.71574707*b3*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3+109.5495979*b4* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2-85.66593514* ... +b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+27.91221430* ... +b0*(z1-3.405800000)^6+.5000000000*d0_0*(-5.658195183+1.954957773* ... +z1)^6-5.864873319*d1*(z1-3.405800000)*(-5.658195183+1.954957773* ... +z1)^5+28.66394920*d2*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4-74.71574707*d3*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3+109.5495979*d4*(z1-3.405800000)^ ... +4*(-5.658195183+1.954957773*z1)^2-85.66593514*d5* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+27.91221430*c0* ... +(z1-3.405800000)^6-1.*z1)*(-5.658195183+1.954957773*z1)^6; + + case 20, + % dz_partial_d1 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = d1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = d0*(-5.658195183+1.954957773*z1)^6; + s(13) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(14) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(15) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(16) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(17) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(18) = b0*(-5.658195183+1.954957773*z1)^6; + s(19) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+ ... +.5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(20) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(21) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(22) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(23) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(24) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(25) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(26) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(27) = d1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(28) = d1_0*(-5.658195183+1.954957773*z1)^5; + s(29) = d0*(-5.658195183+1.954957773*z1)^5; + s(30) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1_0* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-114.6557968*d2* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+448.2944824* ... +d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(31) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(32) = 11.72974664*(-5.658195183+1.954957773*z1)^5+114.6557968* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(33) = -1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6; + s(34) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = b1*(-5.658195183+1.954957773*z1)^5; + s(43) = b0*(-5.658195183+1.954957773*z1)^5; + s(44) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(45) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6; + s(46) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6; + s(47) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6; + s(48) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(49) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(51) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(53) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(55) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(-5.658195183+1.954957773*z1)^5; + s(57) = c0*(-5.658195183+1.954957773*z1)^5; + s(58) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(59) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(60) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(61) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(62) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(63) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(64) = a0*(-5.658195183+1.954957773*z1)^6; + s(65) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(66) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(-5.658195183+1.954957773*z1)^5; + s(75) = a0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(77) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + + dz(1) = 20; + + dz(1) = -1000.*z2/(1901783.760*s(67)-206663.7564*s(52)+52856.32233* ... +s(54)-1901783.760*s(66)+206663.7564*s(53)+1945600.858* ... +s(68)-404018.9169*s(51)-377727.0134*s(37)-5055.520801* ... +s(42)-49416.64843*s(41)+404018.9169*s(50)-394919.9610*s(48)+ ... +5055.520801*s(43)-193214.9219*s(38)+26040.03754*s(75)+ ... +5407.413200*s(57)+394919.9610*s(49)-283554.2453*s(21)+ ... +193214.9219*s(39)+37951.06875*s(27)+478.*cos(s(46))*s(30)+ ... +49416.64843*s(40)-82.*s(30)*cos(s(47))-478.*cos(s(45))*s(30)+ ... +369220.1805*s(35)+148385.4737*s(24)-478.*cos(s(46))*s(44)-82.* ... +s(44)*cos(s(47))+478.*cos(s(45))*s(44)-52856.32233*s(55)-10888.* ... +cos(s(6)-11.72974664*s(5)+57.32789841*s(4)-149.4314941*s(3)+ ... +219.0991957*s(2)-171.3318703*s(1)+55.82442859*d0* ... +(z1-3.405800000)^6)-369220.1805*s(34)-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5-290087.3351* ... +s(22)+82.*s(58)*cos(s(19))+82.*s(44)*cos(s(19))+82.*s(30)* ... +cos(s(19))-1600.*s(76)*sin(s(77))-80.*s(76)*cos(s(77))+10560.+ ... +110867.3152*d5*(z1-3.405800000)^5-110867.3152*c0* ... +(z1-3.405800000)^5-37951.06875*s(26)+164.*s(44)*cos(s(33))+ ... +254535.8689*s(72)-3882.546137*s(29)+3882.546137*s(28)+ ... +283554.2453*s(20)+995213.7509*s(71)-144361.9723*b5* ... +(z1-3.405800000)^5+144361.9723*b0*(z1-3.405800000)^5+290087.3351* ... +s(23)-995213.7509*s(70)-1945600.858*s(69)-478.*cos(s(45))*s(58)+ ... +328.*cos(s(33))+377727.0134*s(36)-26040.03754*s(74)-80.* ... +cos(s(65))-1600.*sin(s(65))+82.*s(58)*cos(s(47))+1600.* ... +sin(s(77))+80.*cos(s(77))-478.*cos(s(46))*s(58)-148385.4737* ... +s(25)-154410.3695*c5*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-5407.413200*s(56)+800.*s(58)*sin(s(65))+40.* ... +s(58)*cos(s(65))+800.*s(58)*sin(s(77))+40.*s(58)*cos(s(77))+ ... +1600.*s(76)*sin(s(65))+80.*s(76)*cos(s(65))-254535.8689*s(73))^2* ... +(-480.9196121*s(58)*sin(s(47))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5+2803.409446*sin(s(46))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5*s(58)+2803.409446*sin(s(45))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(58)-480.9196121*s(58)*sin(s(19))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-3847.356897*sin(s(33))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+3882.546137* ... +(-5.658195183+1.954957773*z1)^5+37951.06875*s(31)-82.*s(32)* ... +cos(s(47))+480.9196121*s(30)*sin(s(47))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+2803.409446*sin(s(45))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5*s(30)-478.* ... +cos(s(45))*s(32)-2803.409446*sin(s(46))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5*s(30)+478.*cos(s(46))*s(32)+ ... +480.9196121*s(44)*sin(s(47))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5-480.9196121*s(44)*sin(s(19))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+2803.409446*sin(s(46))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5* ... +s(44)-2803.409446*sin(s(45))*(z1-3.405800000)*(-5.658195183+ ... +1.954957773*z1)^5*s(44)-1923.678449*s(44)*sin(s(33))* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+82.*s(32)* ... +cos(s(19))-480.9196121*s(30)*sin(s(19))*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5); + + + % dz_partial_d1 (state two) + dz(2) = 20; + + dz(2) = -137.5072333*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +d1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*d2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*d4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*d5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+ ... +23.58910698*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +d1_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*d2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*d4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*d5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^ ... +6+z1)*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + + case 21, + % dz_partial_d2 (state one) + s(1) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = d2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = d0*(-5.658195183+1.954957773*z1)^6; + s(7) = d0*(-5.658195183+1.954957773*z1)^6-11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+57.32789841* ... +d2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-149.4314941*d3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-171.3318703*d5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+55.82442859*c0*(z1-3.405800000)^6; + s(8) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(9) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(10) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(11) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(12) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(13) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(14) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(15) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(16) = b1*(-5.658195183+1.954957773*z1)^5; + s(17) = b0*(-5.658195183+1.954957773*z1)^5; + s(18) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(19) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(20) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(21) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(22) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(23) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(24) = b0*(-5.658195183+1.954957773*z1)^6; + s(25) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(26) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(27) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(28) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(29) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(30) = c0*(-5.658195183+1.954957773*z1)^6; + s(31) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(32) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(33) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(34) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(35) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(36) = (z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(37) = -114.6557968*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +4-448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(38) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(39) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(40) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(41) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(42) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(43) = d2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(44) = d2_0*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(45) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(46) = d1*(-5.658195183+1.954957773*z1)^5; + s(47) = d0*(-5.658195183+1.954957773*z1)^5; + s(48) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2_0* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4-448.2944824* ... +d2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +448.2944824*d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +3+876.3967829*d3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^2-856.6593514*d4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^ ... +5-334.9465716*c0*(z1-3.405800000)^5; + s(49) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = c1*(-5.658195183+1.954957773*z1)^5; + s(58) = c0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(60) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(61) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(62) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(63) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(64) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(65) = a0*(-5.658195183+1.954957773*z1)^6; + s(66) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(67) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = a1*(-5.658195183+1.954957773*z1)^5; + s(76) = a0*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(78) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + + dz(1) = 21; + + dz(1) = -1000.*z2/(-1901783.760*s(67)-404018.9169*s(52)+206663.7564* ... +s(54)-206663.7564*s(53)+1901783.760*s(68)+404018.9169*s(51)+ ... +148385.4737*s(42)+290087.3351*s(41)+394919.9610* ... +s(50)-148385.4737*s(43)-37951.06875*s(44)+283554.2453* ... +s(38)-5407.413200*s(57)-394919.9610*s(49)-3882.546137*s(47)+ ... +5407.413200*s(58)+37951.06875*s(45)+3882.546137*s(46)+ ... +52856.32233*s(55)-110867.3152*c0*(z1-3.405800000)^5+193214.9219* ... +s(13)+110867.3152*d5*(z1-3.405800000)^5-52856.32233* ... +s(56)-290087.3351*s(40)-283554.2453*s(39)-1945600.858*s(70)+ ... +254535.8689*s(73)-995213.7509*s(71)-26040.03754* ... +s(75)-254535.8689*s(74)+995213.7509*s(72)+1945600.858*s(69)+ ... +26040.03754*s(76)+49416.64843*s(14)+5055.520801*s(17)-478.* ... +cos(s(32))*s(59)+164.*s(18)*cos(s(7))+478.*cos(s(32))* ... +s(48)-1600.*s(77)*sin(s(78))-80.*s(77)*cos(s(78))-478.* ... +cos(s(31))*s(48)+82.*s(59)*cos(s(33))+82.*s(48)*cos(s(33))-82.* ... +s(48)*cos(s(34))-478.*cos(s(32))*s(18)-82.*s(18)*cos(s(34))+478.* ... +cos(s(31))*s(18)-478.*cos(s(31))*s(59)+82.*s(59)*cos(s(34))+82.* ... +s(18)*cos(s(33))+10560.+144361.9723*b0*(z1-3.405800000)^5+80.* ... +s(77)*cos(s(66))+1600.*s(77)*sin(s(66))+40.*s(59)*cos(s(78))+ ... +800.*s(59)*sin(s(78))+800.*s(59)*sin(s(66))+40.*s(59)* ... +cos(s(66))-144361.9723*b5*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-154410.3695*c5*(z1-3.405800000)^5-10888.* ... +cos(s(30)-11.72974664*s(29)+57.32789841*s(28)-149.4314941*s(27)+ ... +219.0991957*s(26)-171.3318703*s(25)+55.82442859*d0* ... +(z1-3.405800000)^6)+377727.0134*s(10)-5055.520801*s(16)+ ... +369220.1805*s(9)-377727.0134*s(11)-369220.1805*s(8)+328.* ... +cos(s(7))-193214.9219*s(12)-49416.64843*s(15)+80.*cos(s(78))+ ... +1600.*sin(s(78))-80.*cos(s(66))-1600.*sin(s(66))-743581.3889*a5* ... +(z1-3.405800000)^5+743581.3889*a0*(z1-3.405800000)^5)^2* ... +(-18803.55068*sin(s(7))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4-2350.443835*s(59)*sin(s(34))*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-2350.443835*s(59)*sin(s(33))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+13701.36772* ... +sin(s(31))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4* ... +s(59)+13701.36772*sin(s(32))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(59)-37951.06875*s(36)-148385.4737*s(35)+82.* ... +s(37)*cos(s(33))-2350.443835*s(48)*sin(s(33))*(z1-3.405800000)^2* ... +(-5.658195183+1.954957773*z1)^4-82.*s(37)*cos(s(34))+2350.443835* ... +s(48)*sin(s(34))*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+13701.36772*sin(s(31))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(48)-478.*cos(s(31))*s(37)-13701.36772* ... +sin(s(32))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4* ... +s(48)+478.*cos(s(32))*s(37)+2350.443835*s(18)*sin(s(34))* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-2350.443835* ... +s(18)*sin(s(33))*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+13701.36772*sin(s(32))*(z1-3.405800000)^2*(-5.658195183+ ... +1.954957773*z1)^4*s(18)-13701.36772*sin(s(31))*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4*s(18)-9401.775339*s(18)* ... +sin(s(7))*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4); + + + % dz_partial_d2 (state two) + dz(2) = 21; + + dz(2) = -672.0520866*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +d2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*d5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*c0*(z1-3.405800000)^6+ ... +z1)*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +115.2892701*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +d2_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*d3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*d5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*c0*(z1-3.405800000)^ ... +6-1.*z1)*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + + case 22, + % dz_partial_d3 (state one) + s(1) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = d3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = d0*(-5.658195183+1.954957773*z1)^6; + s(7) = -1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6; + s(8) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(9) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(10) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(11) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(12) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(13) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(14) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(15) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(16) = b1*(-5.658195183+1.954957773*z1)^5; + s(17) = b0*(-5.658195183+1.954957773*z1)^5; + s(18) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(19) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(20) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(21) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(22) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(23) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(24) = b0*(-5.658195183+1.954957773*z1)^6; + s(25) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(26) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(27) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(28) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(29) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(30) = c0*(-5.658195183+1.954957773*z1)^6; + s(31) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(32) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(33) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(34) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(35) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(37) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(38) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(39) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(40) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(41) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(42) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(43) = c1*(-5.658195183+1.954957773*z1)^5; + s(44) = c0*(-5.658195183+1.954957773*z1)^5; + s(45) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(46) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(47) = (z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(48) = 448.2944824*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+ ... +876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(49) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = d3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = d3_0*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = d1*(-5.658195183+1.954957773*z1)^5; + s(58) = d0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3_0* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(60) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(61) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(62) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(63) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(64) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(65) = a0*(-5.658195183+1.954957773*z1)^6; + s(66) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(67) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = a1*(-5.658195183+1.954957773*z1)^5; + s(76) = a0*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(78) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + + dz(1) = 22; + + dz(1) = -1000.*z2/(-1901783.760*s(67)+290087.3351*s(52)-148385.4737* ... +s(54)+148385.4737*s(53)+1901783.760*s(68)-290087.3351*s(51)+ ... +404018.9169*s(37)-52856.32233*s(42)+52856.32233* ... +s(41)-283554.2453*s(50)-5407.413200*s(43)+5407.413200* ... +s(44)-404018.9169*s(38)+3882.546137*s(57)+283554.2453* ... +s(49)-3882.546137*s(58)-743581.3889*a5*(z1-3.405800000)^5+ ... +743581.3889*a0*(z1-3.405800000)^5+193214.9219*s(13)-394919.9610* ... +s(35)+49416.64843*s(14)-154410.3695*c5*(z1-3.405800000)^5+ ... +154410.3695*d0*(z1-3.405800000)^5+26040.03754*s(76)+995213.7509* ... +s(72)+254535.8689*s(73)-26040.03754*s(75)-254535.8689*s(74)+ ... +5055.520801*s(17)-995213.7509*s(71)+37951.06875* ... +s(56)-37951.06875*s(55)-1945600.858*s(70)+1945600.858* ... +s(69)-206663.7564*s(39)+206663.7564*s(40)+10560.-144361.9723*b5* ... +(z1-3.405800000)^5+144361.9723*b0*(z1-3.405800000)^5+800.*s(45)* ... +sin(s(66))+82.*s(45)*cos(s(34))-82.*s(59)*cos(s(34))+1600.*s(77)* ... +sin(s(66))+80.*s(77)*cos(s(66))+40.*s(45)*cos(s(66))+82.*s(18)* ... +cos(s(33))-478.*cos(s(31))*s(45)-478.*cos(s(32))*s(18)+82.*s(45)* ... +cos(s(33))+800.*s(45)*sin(s(78))+40.*s(45)*cos(s(78))+164.*s(18)* ... +cos(s(7))-478.*cos(s(31))*s(59)-82.*s(18)*cos(s(34))+82.*s(59)* ... +cos(s(33))+478.*cos(s(31))*s(18)+478.*cos(s(32))*s(59)-478.* ... +cos(s(32))*s(45)-80.*s(77)*cos(s(78))-1600.*s(77)* ... +sin(s(78))-10888.*cos(s(30)-11.72974664*s(29)+57.32789841* ... +s(28)-149.4314941*s(27)+219.0991957*s(26)-171.3318703*s(25)+ ... +55.82442859*d0*(z1-3.405800000)^6)+377727.0134*s(10)-110867.3152* ... +c0*(z1-3.405800000)^5+110867.3152*d5*(z1-3.405800000)^ ... +5-5055.520801*s(16)+369220.1805*s(9)-377727.0134* ... +s(11)-369220.1805*s(8)+328.*cos(s(7))-193214.9219*s(12)+ ... +394919.9610*s(36)-49416.64843*s(15)+80.*cos(s(78))+1600.* ... +sin(s(78))-80.*cos(s(66))-1600.*sin(s(66)))^2*(-6126.691259* ... +s(45)*sin(s(33))*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+35714.12710*sin(s(31))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(45)+148385.4737*s(47)-49013.53007*sin(s(7))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+35714.12710* ... +sin(s(32))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3* ... +s(45)+290087.3351*s(46)+82.*s(48)*cos(s(33))-6126.691259*s(59)* ... +sin(s(33))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-82.*s(48)*cos(s(34))+6126.691259*s(59)*sin(s(34))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+35714.12710* ... +sin(s(31))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3* ... +s(59)-478.*cos(s(31))*s(48)-35714.12710*sin(s(32))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3*s(59)+478.* ... +cos(s(32))*s(48)-6126.691259*s(45)*sin(s(34))*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3+6126.691259*s(18)*sin(s(34))* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-6126.691259* ... +s(18)*sin(s(33))*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+35714.12710*sin(s(32))*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3*s(18)-35714.12710*sin(s(31))*(z1-3.405800000)^ ... +3*(-5.658195183+1.954957773*z1)^3*s(18)-24506.76504*s(18)* ... +sin(s(7))*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3); + + + % dz_partial_d3 (state two) + dz(2) = 22; + + dz(2) = -1751.777934*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-1.*z1)* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+300.5142063* ... +sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3_0* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+z1)*(z1-3.405800000)^3* ... +(-5.658195183+1.954957773*z1)^3; + + case 23, + % dz_partial_d4 (state one) + s(1) = d5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = d0*(-5.658195183+1.954957773*z1)^6; + s(7) = d0*(-5.658195183+1.954957773*z1)^6-11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +219.0991957*d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-171.3318703*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+55.82442859*c0*(z1-3.405800000)^6; + s(8) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(9) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(10) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(11) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(12) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(13) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(14) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(15) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(16) = b1*(-5.658195183+1.954957773*z1)^5; + s(17) = b0*(-5.658195183+1.954957773*z1)^5; + s(18) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(19) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(20) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(21) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(22) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(23) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(24) = b0*(-5.658195183+1.954957773*z1)^6; + s(25) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(26) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(27) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(28) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(29) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(30) = c0*(-5.658195183+1.954957773*z1)^6; + s(31) = -.5000000000*c0*(-5.658195183+1.954957773*z1)^6+5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*d0*(z1-3.405800000)^6+2.* ... +z1-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6; + s(32) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(33) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(34) = (z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(35) = -876.3967829*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(36) = d5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(37) = d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(38) = d4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(39) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(40) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(41) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(42) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(43) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(44) = d1*(-5.658195183+1.954957773*z1)^5; + s(45) = d0*(-5.658195183+1.954957773*z1)^5; + s(46) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4_0*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4_0*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*d5*(z1-3.405800000)^5-334.9465716*c0* ... +(z1-3.405800000)^5; + s(47) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(48) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6+.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(49) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(51) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(53) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(55) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(57) = c1*(-5.658195183+1.954957773*z1)^5; + s(58) = c0*(-5.658195183+1.954957773*z1)^5; + s(59) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(60) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(61) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(62) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(63) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(64) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(65) = a0*(-5.658195183+1.954957773*z1)^6; + s(66) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(67) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(69) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(71) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(73) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(75) = a1*(-5.658195183+1.954957773*z1)^5; + s(76) = a0*(-5.658195183+1.954957773*z1)^5; + s(77) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(78) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + + dz(1) = 23; + + dz(1) = -1000.*z2/(-1901783.760*s(67)-404018.9169*s(52)-1945600.858* ... +s(70)+206663.7564*s(54)-206663.7564*s(53)+1901783.760*s(68)+ ... +404018.9169*s(51)-283554.2453*s(37)-37951.06875* ... +s(42)-148385.4737*s(41)+995213.7509*s(72)+394919.9610*s(50)+ ... +26040.03754*s(76)+37951.06875*s(43)+3882.546137* ... +s(44)-290087.3351*s(38)-254535.8689*s(74)-995213.7509* ... +s(71)-5407.413200*s(57)-394919.9610*s(49)-26040.03754*s(75)+ ... +254535.8689*s(73)+5407.413200*s(58)-3882.546137*s(45)+ ... +290087.3351*s(39)+193214.9219*s(13)+49416.64843*s(14)+ ... +5055.520801*s(17)+110867.3152*d5*(z1-3.405800000)^5-110867.3152* ... +c0*(z1-3.405800000)^5-144361.9723*b5*(z1-3.405800000)^5+ ... +144361.9723*b0*(z1-3.405800000)^5-10888.*cos(s(30)-11.72974664* ... +s(29)+57.32789841*s(28)-149.4314941*s(27)+219.0991957* ... +s(26)-171.3318703*s(25)+55.82442859*d0*(z1-3.405800000)^6)+ ... +377727.0134*s(10)+478.*cos(s(32))*s(46)+478.*cos(s(31))*s(18)+ ... +82.*s(18)*cos(s(48))-82.*s(18)*cos(s(47))-478.*cos(s(32))* ... +s(18)-154410.3695*c5*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-5055.520801*s(16)-478.*cos(s(32))*s(59)+ ... +369220.1805*s(9)+164.*s(18)*cos(s(7))+148385.4737* ... +s(40)-743581.3889*a5*(z1-3.405800000)^5+743581.3889*a0* ... +(z1-3.405800000)^5+10560.-377727.0134*s(11)-478.*cos(s(31))* ... +s(46)-369220.1805*s(8)+328.*cos(s(7))-1600.*s(77)*sin(s(78))-80.* ... +s(77)*cos(s(78))-478.*cos(s(31))*s(59)-193214.9219*s(12)+82.* ... +s(46)*cos(s(48))-82.*s(46)*cos(s(47))+283554.2453* ... +s(36)-49416.64843*s(15)+82.*s(59)*cos(s(48))+82.*s(59)* ... +cos(s(47))+1945600.858*s(69)+52856.32233*s(55)+800.*s(59)* ... +sin(s(78))+40.*s(59)*cos(s(78))+800.*s(59)*sin(s(66))+40.*s(59)* ... +cos(s(66))-52856.32233*s(56)+80.*cos(s(78))+1600.*s(77)* ... +sin(s(66))+80.*s(77)*cos(s(66))+1600.*sin(s(78))-80.* ... +cos(s(66))-1600.*sin(s(66)))^2*(-71864.53620*sin(s(7))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2-290087.3351* ... +s(34)-283554.2453*s(33)-8983.067025*s(59)*sin(s(47))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+52364.70778* ... +sin(s(32))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2* ... +s(59)-8983.067025*s(59)*sin(s(48))*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+52364.70778*sin(s(31))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2* ... +s(59)-52364.70778*sin(s(32))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(46)+478.*cos(s(32))*s(35)+8983.067025*s(18)* ... +sin(s(47))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-8983.067025*s(18)*sin(s(48))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2+82.*s(35)*cos(s(48))-8983.067025*s(46)* ... +sin(s(48))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-82.*s(35)*cos(s(47))+8983.067025*s(46)*sin(s(47))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+52364.70778* ... +sin(s(31))*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2* ... +s(46)-478.*cos(s(31))*s(35)+52364.70778*sin(s(32))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2* ... +s(18)-52364.70778*sin(s(31))*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2*s(18)-35932.26810*s(18)*sin(s(7))* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2); + + + % dz_partial_d4 (state two) + dz(2) = 23; + + dz(2) = -2568.488917*sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+ ... +5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5-28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4+74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3-109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2+85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^ ... +6+.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+27.91221430*c0*(z1-3.405800000)^6+z1)* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+440.6194376* ... +sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^ ... +4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^3+109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^6+ ... +.5000000000*d0*(-5.658195183+1.954957773*z1)^6-5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*d4_0*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2-85.66593514*d5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)+27.91221430*c0*(z1-3.405800000)^6-1.*z1)* ... +(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + + case 24, + % dz_partial_d5 (state one) + s(1) = c5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(2) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(3) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(4) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(5) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(6) = c0*(-5.658195183+1.954957773*z1)^6; + s(7) = d5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(8) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(9) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(10) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(11) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(12) = d0*(-5.658195183+1.954957773*z1)^6; + s(13) = b5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(14) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(15) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(16) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(17) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(18) = b0*(-5.658195183+1.954957773*z1)^6; + s(19) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6; + s(20) = c5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(21) = c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(22) = c4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(23) = c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(24) = c3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(25) = c2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(26) = c2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(27) = c1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(28) = c1*(-5.658195183+1.954957773*z1)^5; + s(29) = c0*(-5.658195183+1.954957773*z1)^5; + s(30) = -11.72974664*c0*(-5.658195183+1.954957773*z1)^5+11.72974664*c1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*c2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*c3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*c4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*c4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*c5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*c5*(z1-3.405800000)^5-334.9465716*d0* ... +(z1-3.405800000)^5; + s(31) = -.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(32) = -1.*d0*(-5.658195183+1.954957773*z1)^6+11.72974664*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*d5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*c0*(z1-3.405800000)^6; + s(33) = b5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(34) = b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(35) = b4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(36) = b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(37) = b3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(38) = b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(39) = b2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(40) = b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(41) = b1*(-5.658195183+1.954957773*z1)^5; + s(42) = b0*(-5.658195183+1.954957773*z1)^5; + s(43) = -11.72974664*b0*(-5.658195183+1.954957773*z1)^5+11.72974664*b1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*b2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*b3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*b4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*b4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*b5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*b5*(z1-3.405800000)^5-334.9465716*b0* ... +(z1-3.405800000)^5; + s(44) = .5000000000*c0*(-5.658195183+1.954957773*z1)^6-5.864873319*c1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*c2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +c3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6-2.*z1+.5000000000*b0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*b1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*b2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*b3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6; + s(45) = .5000000000*b0*(-5.658195183+1.954957773*z1)^6-5.864873319*b1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5+28.66394920*b2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4-74.71574707* ... +b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+ ... +109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6-2.*z1-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6; + s(46) = (z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(47) = 856.6593514*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)+ ... +334.9465716*(z1-3.405800000)^5; + s(48) = d5_0*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(49) = d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(50) = d4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(51) = d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(52) = d3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(53) = d2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(54) = d2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(55) = d1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(56) = d1*(-5.658195183+1.954957773*z1)^5; + s(57) = d0*(-5.658195183+1.954957773*z1)^5; + s(58) = -11.72974664*d0*(-5.658195183+1.954957773*z1)^5+11.72974664*d1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*d2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*d3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*d4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*d4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*d5_0*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)+334.9465716*d5_0*(z1-3.405800000)^ ... +5-334.9465716*c0*(z1-3.405800000)^5; + s(59) = a5*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1); + s(60) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2; + s(61) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3; + s(62) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4; + s(63) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5; + s(64) = a0*(-5.658195183+1.954957773*z1)^6; + s(65) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6+.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6-5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5+28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4-74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3+109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^ ... +2-85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)+27.91221430*d0*(z1-3.405800000)^6+z1; + s(66) = a5*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(67) = a4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1); + s(68) = a4*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(69) = a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^2; + s(70) = a3*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(71) = a2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3; + s(72) = a2*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(73) = a1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^4; + s(74) = a1*(-5.658195183+1.954957773*z1)^5; + s(75) = a0*(-5.658195183+1.954957773*z1)^5; + s(76) = -11.72974664*a0*(-5.658195183+1.954957773*z1)^5+11.72974664*a1* ... +(-5.658195183+1.954957773*z1)^5+114.6557968*a1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-114.6557968*a2*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^4-448.2944824*a2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^3+448.2944824*a3* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^3+876.3967829* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +2-876.3967829*a4*(z1-3.405800000)^3*(-5.658195183+1.954957773* ... +z1)^2-856.6593514*a4*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+856.6593514*a5*(z1-3.405800000)^4*(-5.658195183+ ... +1.954957773*z1)+334.9465716*a5*(z1-3.405800000)^5-334.9465716*a0* ... +(z1-3.405800000)^5; + s(77) = -1.*a0*(-5.658195183+1.954957773*z1)^6+11.72974664*a1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-57.32789841*a2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+149.4314941* ... +a3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-219.0991957*a4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+171.3318703*a5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-55.82442859*a0*(z1-3.405800000)^6-.5000000000*c0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*c1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*c2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*c3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +c4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*c5*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*d0*(z1-3.405800000)^6+z1; + + dz(1) = 24; + + dz(1) = -1000.*z2/(1901783.760*s(67)+148385.4737*s(52)-37951.06875* ... +s(54)-1901783.760*s(66)-148385.4737*s(53)+1945600.858*s(68)+ ... +290087.3351*s(51)-193214.9219*s(37)+5055.520801* ... +s(42)-995213.7509*s(70)+995213.7509*s(71)-5055.520801* ... +s(41)-290087.3351*s(50)+283554.2453*s(48)+26040.03754* ... +s(75)-26040.03754*s(74)+193214.9219*s(38)-1945600.858* ... +s(69)-3882.546137*s(57)-283554.2453*s(49)-254535.8689*s(73)+ ... +254535.8689*s(72)+394919.9610*s(21)-52856.32233*s(27)+ ... +377727.0134*s(35)-206663.7564*s(24)+49416.64843* ... +s(39)-154410.3695*c5*(z1-3.405800000)^5+154410.3695*d0* ... +(z1-3.405800000)^5-10888.*cos(s(6)-11.72974664*s(5)+57.32789841* ... +s(4)-149.4314941*s(3)+219.0991957*s(2)-171.3318703*s(1)+ ... +55.82442859*d0*(z1-3.405800000)^6)+369220.1805*s(34)+3882.546137* ... +s(56)+37951.06875*s(55)+404018.9169*s(22)+328.* ... +cos(s(32))-144361.9723*b5*(z1-3.405800000)^5+144361.9723*b0* ... +(z1-3.405800000)^5-49416.64843*s(40)+164.*s(43)*cos(s(32))+ ... +52856.32233*s(26)+110867.3152*d5_0*(z1-3.405800000)^ ... +5-110867.3152*c0*(z1-3.405800000)^5+82.*s(30)*cos(s(19))+82.* ... +s(58)*cos(s(19))+82.*s(43)*cos(s(19))+5407.413200* ... +s(29)-5407.413200*s(28)-394919.9610*s(20)-80.*s(76)* ... +cos(s(77))-743581.3889*a5*(z1-3.405800000)^5+743581.3889*a0* ... +(z1-3.405800000)^5-404018.9169*s(23)-1600.*s(76)*sin(s(77))+ ... +10560.-377727.0134*s(36)+478.*cos(s(44))*s(43)-478.*cos(s(45))* ... +s(43)-369220.1805*s(33)-80.*cos(s(65))-1600.*sin(s(65))+478.* ... +cos(s(45))*s(58)+1600.*sin(s(77))+80.*cos(s(77))-82.*s(58)* ... +cos(s(31))-82.*s(43)*cos(s(31))+206663.7564*s(25)-478.* ... +cos(s(44))*s(58)+1600.*s(76)*sin(s(65))-478.*cos(s(44))*s(30)+ ... +82.*s(30)*cos(s(31))+40.*s(30)*cos(s(77))+800.*s(30)* ... +sin(s(77))-478.*cos(s(45))*s(30)+800.*s(30)*sin(s(65))+40.*s(30)* ... +cos(s(65))+80.*s(76)*cos(s(65)))^2*(-56196.85345*sin(s(32))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+40948.31700* ... +sin(s(44))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)* ... +s(30)+40948.31700*sin(s(45))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)*s(30)+283554.2453*s(46)+110867.3152* ... +(z1-3.405800000)^5-82.*s(47)*cos(s(31))+7024.606682*s(58)* ... +sin(s(31))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+ ... +40948.31700*sin(s(44))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)*s(58)-478.*cos(s(44))*s(47)+82.*s(47)* ... +cos(s(19))-7024.606682*s(58)*sin(s(19))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-40948.31700*sin(s(45))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)*s(58)+478.* ... +cos(s(45))*s(47)+7024.606682*s(43)*sin(s(31))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)-7024.606682*s(43)*sin(s(19))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+40948.31700* ... +sin(s(45))*(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)* ... +s(43)-40948.31700*sin(s(44))*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)*s(43)-28098.42673*s(43)*sin(s(32))* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)-7024.606682* ... +s(30)*sin(s(31))*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-7024.606682*s(30)*sin(s(19))*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1)); + + + % dz_partial_d5 (state two) + dz(2) = 24; + + dz(2) = -2008.514949*sin(.5000000000*b0*(-5.658195183+1.954957773*z1)^ ... +6-5.864873319*b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^ ... +5+28.66394920*b2*(z1-3.405800000)^2*(-5.658195183+1.954957773* ... +z1)^4-74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+ ... +1.954957773*z1)^3+109.5495979*b4*(z1-3.405800000)^4* ... +(-5.658195183+1.954957773*z1)^2-85.66593514*b5*(z1-3.405800000)^ ... +5*(-5.658195183+1.954957773*z1)+27.91221430*b0*(z1-3.405800000)^ ... +6-.5000000000*d0*(-5.658195183+1.954957773*z1)^6+5.864873319*d1* ... +(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920*d2* ... +(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+74.71574707* ... +d3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*d4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*d5_0*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*c0*(z1-3.405800000)^6-1.*z1)* ... +(z1-3.405800000)^5*(-5.658195183+1.954957773*z1)+344.5569577* ... +sin(-.5000000000*b0*(-5.658195183+1.954957773*z1)^6+5.864873319* ... +b1*(z1-3.405800000)*(-5.658195183+1.954957773*z1)^5-28.66394920* ... +b2*(z1-3.405800000)^2*(-5.658195183+1.954957773*z1)^4+ ... +74.71574707*b3*(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^ ... +3-109.5495979*b4*(z1-3.405800000)^4*(-5.658195183+1.954957773* ... +z1)^2+85.66593514*b5*(z1-3.405800000)^5*(-5.658195183+ ... +1.954957773*z1)-27.91221430*b0*(z1-3.405800000)^6-.5000000000*d0* ... +(-5.658195183+1.954957773*z1)^6+5.864873319*d1*(z1-3.405800000)* ... +(-5.658195183+1.954957773*z1)^5-28.66394920*d2*(z1-3.405800000)^ ... +2*(-5.658195183+1.954957773*z1)^4+74.71574707*d3* ... +(z1-3.405800000)^3*(-5.658195183+1.954957773*z1)^3-109.5495979* ... +d4*(z1-3.405800000)^4*(-5.658195183+1.954957773*z1)^2+ ... +85.66593514*d5_0*(z1-3.405800000)^5*(-5.658195183+1.954957773* ... +z1)-27.91221430*c0*(z1-3.405800000)^6+z1)*(z1-3.405800000)^5* ... +(-5.658195183+1.954957773*z1); + + otherwise + error('opt parameter out of range'); +end diff --git a/demos/rabbit/anim/rabbit_model/zd_diff_cc.m b/demos/rabbit/anim/rabbit_model/zd_diff_cc.m new file mode 100644 index 0000000..6cbbd90 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_diff_cc.m @@ -0,0 +1,801 @@ +function dz = zd_diff_cc(z,opt) +%ZD_DIFF_CC + +%Eric Westervelt +%26-Feb-2001 23:39:59 + +[a,b,m] = controlParameters; + +a0=a(5); a1=a(6); a2=a(7); a3=a(8); a4=a(9); a5=a(10); +b0=a(11); b1=a(12); b2=a(13); b3=a(14); b4=a(15); b5=a(16); +c0=a(17); c1=a(18); c2=a(19); c3=a(20); c4=a(21); c5=a(22); +d0=a(23); d1=a(24); d2=a(25); d3=a(26); d4=a(27); d5=a(28); + +a0_0=a(5); a1_0=a(6); a2_0=a(7); a3_0=a(8); a4_0=a(9); a5_0=a(10); +b0_0=a(11); b1_0=a(12); b2_0=a(13); b3_0=a(14); b4_0=a(15); b5_0=a(16); +c0_0=a(17); c1_0=a(18); c2_0=a(19); c3_0=a(20); c4_0=a(21); c5_0=a(22); +d0_0=a(23); d1_0=a(24); d2_0=a(25); d3_0=a(26); d4_0=a(27); d5_0=a(28); + +z1 = z(1); z2 = z(2); + +s(1) = b5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(2) = b4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(3) = b4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(4) = b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(5) = b3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(6) = b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(7) = b2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(8) = b1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(9) = c5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(10) = c4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(11) = c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(12) = c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(13) = c1*(z1-b)/m*(1-(z1-b)/m)^5; +s(14) = d5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(15) = d4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(16) = d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(17) = d2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(18) = d1*(z1-b)/m*(1-(z1-b)/m)^5; +s(19) = b5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(20) = b4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(21) = b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(22) = b2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(23) = b1*(z1-b)/m*(1-(z1-b)/m)^5; +s(24) = cos(-1/2*b0*(1-(z1-b)/m)^6-3*b1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*b2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*b0* ... +(z1-b)^6/m^6+1/2*d0*(1-(z1-b)/m)^6+3*d1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*c0* ... +(z1-b)^6/m^6+2*z1+1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6); +s(25) = a5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(26) = a1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(27) = a2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(28) = a2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(29) = a3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(30) = a3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(31) = a4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(32) = a4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(33) = d5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(34) = d4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(35) = d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(36) = d3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(37) = d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(38) = d2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(39) = c5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(40) = c4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(41) = c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(42) = c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(43) = c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(44) = c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(45) = c2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(46) = c1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(47) = cos(1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6-2*z1+1/2*b0*(1-(z1-b)/m)^6+3*b1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +b2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*b0* ... +(z1-b)^6/m^6-1/2*d0*(1-(z1-b)/m)^6-3*d1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*c0* ... +(z1-b)^6/m^6); +s(48) = d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(49) = d1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(50) = a5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(51) = a4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(52) = a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(53) = a2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(54) = a1*(z1-b)/m*(1-(z1-b)/m)^5; +s(55) = -a0*(1-(z1-b)/m)^6-6*a1*(z1-b)/m*(1-(z1-b)/m)^5-15*a2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-20*a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15*a4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-6*a5*(z1-b)^5/m^5*(1-(z1-b)/m)-a0*(z1-b)^6/m^6+1/2*c0* ... +(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6+z1; +s(56) = cos(1/2*b0*(1-(z1-b)/m)^6+3*b1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*b2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*b0* ... +(z1-b)^6/m^6+1/2*d0*(1-(z1-b)/m)^6+3*d1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*c0* ... +(z1-b)^6/m^6-2*z1+1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6); +s(57) = -a0*(1-(z1-b)/m)^6-6*a1*(z1-b)/m*(1-(z1-b)/m)^5-15*a2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-20*a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15*a4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-6*a5*(z1-b)^5/m^5*(1-(z1-b)/m)-a0*(z1-b)^6/m^6-1/2*c0* ... +(1-(z1-b)/m)^6-3*c1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*d0*(z1-b)^6/m^6+z1; +s(58) = cos(-1/2*b0*(1-(z1-b)/m)^6-3*b1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*b2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*b0* ... +(z1-b)^6/m^6-1/2*d0*(1-(z1-b)/m)^6-3*d1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*c0* ... +(z1-b)^6/m^6+2*z1+1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6); + +% alpha +alpha = 1000/(-2586*b5*(z1-b)^5/m^6-1986*d1/m*(1-(z1-b)/m)^5+2586*b0* ... +(z1-b)^5/m^6+13320*a1/m*(1-(z1-b)/m)^5-2766*c0*(1-(z1-b)/m)^5/m-25860* ... +s(4)+2586*b1/m*(1-(z1-b)/m)^5-2586*b0*(1-(z1-b)/m)^5/m+1986*d5* ... +(z1-b)^5/m^6-2766*c5*(z1-b)^5/m^6-(492*b0*(1-(z1-b)/m)^5/m-492*b1/m* ... +(1-(z1-b)/m)^5+2460*s(8)-2460*s(7)+4920*s(6)-4920*s(5)+4920*s(4)-4920* ... +s(3)+2460*s(2)-2460*s(1)+492*b5*(z1-b)^5/m^6-492*b0*(z1-b)^5/m^6)* ... +s(56)+(984*b0*(1-(z1-b)/m)^5/m-984*b1/m*(1-(z1-b)/m)^5+4920*s(8)-4920* ... +s(7)+9840*s(6)-9840*s(5)+9840*s(4)-9840*s(3)+4920*s(2)-4920*s(1)+984*b5* ... +(z1-b)^5/m^6-984*b0*(z1-b)^5/m^6)*cos(d0*(1-(z1-b)/m)^6+6*s(18)+15* ... +s(17)+20*s(16)+15*s(15)+6*s(14)+c0*(z1-b)^6/m^6)+478*s(47)*(6*b0* ... +(1-(z1-b)/m)^5/m-6*b1/m*(1-(z1-b)/m)^5+30*s(8)-30*s(7)+60*s(6)-60*s(5)+60* ... +s(4)-60*s(3)+30*s(2)-30*s(1)+6*b5*(z1-b)^5/m^6-6*b0*(z1-b)^5/m^6)+(492*b0* ... +(1-(z1-b)/m)^5/m-492*b1/m*(1-(z1-b)/m)^5+2460*s(8)-2460*s(7)+4920* ... +s(6)-4920*s(5)+4920*s(4)-4920*s(3)+2460*s(2)-2460*s(1)+492*b5* ... +(z1-b)^5/m^6-492*b0*(z1-b)^5/m^6)*s(58)-478*s(24)*(6*b0*(1-(z1-b)/m)^5/m-6* ... +b1/m*(1-(z1-b)/m)^5+30*s(8)-30*s(7)+60*s(6)-60*s(5)+60*s(4)-60*s(3)+30* ... +s(2)-30*s(1)+6*b5*(z1-b)^5/m^6-6*b0*(z1-b)^5/m^6)+27660*s(41)-13320*a0* ... +(1-(z1-b)/m)^5/m-1986*c0*(z1-b)^5/m^6+133200*s(31)+1986*d0* ... +(1-(z1-b)/m)^5/m+2766*d0*(z1-b)^5/m^6+19860*s(48)+12930*s(1)+13320*a0* ... +(z1-b)^5/m^6-13320*a5*(z1-b)^5/m^6+2766*c1/m*(1-(z1-b)/m)^5+13830*s(39)-80* ... +cos(s(55))+133200*s(29)+10560+66600*s(25)-10888*cos(c0*(1-(z1-b)/m)^6+6* ... +s(13)+15*s(12)+20*s(11)+15*s(10)+6*s(9)+d0*(z1-b)^6/m^6)-66600*s(32)-1600* ... +sin(s(55))-19860*s(36)+80*cos(s(57))+1600*sin(s(57))-27660*s(44)-12930* ... +s(8)+13830*s(45)-27660*s(42)+328*cos(d0*(1-(z1-b)/m)^6+6*s(18)+15*s(17)+20* ... +s(16)+15*s(15)+6*s(14)+c0*(z1-b)^6/m^6)-13830*s(40)+(4800*c0* ... +(1-(z1-b)/m)^5/m-4800*c1/m*(1-(z1-b)/m)^5+24000*s(46)-24000*s(45)+48000* ... +s(44)-48000*s(43)+48000*s(42)-48000*s(41)+24000*s(40)-24000*s(39)+4800*c5* ... +(z1-b)^5/m^6-4800*d0*(z1-b)^5/m^6)*sin(s(57))+25860*s(5)-25860*s(6)-133200* ... +s(30)+66600*s(27)+19860*s(37)-12930*s(2)+9930*s(34)+9930*s(49)-478*s(47)* ... +(6*c0*(1-(z1-b)/m)^5/m-6*c1/m*(1-(z1-b)/m)^5+30*s(46)-30*s(45)+60*s(44)-60* ... +s(43)+60*s(42)-60*s(41)+30*s(40)-30*s(39)+6*c5*(z1-b)^5/m^6-6*d0* ... +(z1-b)^5/m^6)+(240*c0*(1-(z1-b)/m)^5/m-240*c1/m*(1-(z1-b)/m)^5+1200* ... +s(46)-1200*s(45)+2400*s(44)-2400*s(43)+2400*s(42)-2400*s(41)+1200* ... +s(40)-1200*s(39)+240*c5*(z1-b)^5/m^6-240*d0*(z1-b)^5/m^6)*cos(s(57))+(240* ... +c0*(1-(z1-b)/m)^5/m-240*c1/m*(1-(z1-b)/m)^5+1200*s(46)-1200*s(45)+2400* ... +s(44)-2400*s(43)+2400*s(42)-2400*s(41)+1200*s(40)-1200*s(39)+240*c5* ... +(z1-b)^5/m^6-240*d0*(z1-b)^5/m^6)*cos(s(55))+(492*c0*(1-(z1-b)/m)^5/m-492* ... +c1/m*(1-(z1-b)/m)^5+2460*s(46)-2460*s(45)+4920*s(44)-4920*s(43)+4920* ... +s(42)-4920*s(41)+2460*s(40)-2460*s(39)+492*c5*(z1-b)^5/m^6-492*d0* ... +(z1-b)^5/m^6)*s(58)+(4800*c0*(1-(z1-b)/m)^5/m-4800*c1/m* ... +(1-(z1-b)/m)^5+24000*s(46)-24000*s(45)+48000*s(44)-48000*s(43)+48000* ... +s(42)-48000*s(41)+24000*s(40)-24000*s(39)+4800*c5*(z1-b)^5/m^6-4800*d0* ... +(z1-b)^5/m^6)*sin(s(55))+(492*c0*(1-(z1-b)/m)^5/m-492*c1/m* ... +(1-(z1-b)/m)^5+2460*s(46)-2460*s(45)+4920*s(44)-4920*s(43)+4920*s(42)-4920* ... +s(41)+2460*s(40)-2460*s(39)+492*c5*(z1-b)^5/m^6-492*d0*(z1-b)^5/m^6)* ... +s(56)-133200*s(28)-66600*s(26)-19860*s(35)-9930*s(33)+25860*s(3)+478*s(24)* ... +(6*d0*(1-(z1-b)/m)^5/m-6*d1/m*(1-(z1-b)/m)^5+30*s(49)-30*s(38)+60*s(37)-60* ... +s(36)+60*s(48)-60*s(35)+30*s(34)-30*s(33)+6*d5*(z1-b)^5/m^6-6*c0* ... +(z1-b)^5/m^6)-478*s(24)*(6*c0*(1-(z1-b)/m)^5/m-6*c1/m*(1-(z1-b)/m)^5+30* ... +s(46)-30*s(45)+60*s(44)-60*s(43)+60*s(42)-60*s(41)+30*s(40)-30*s(39)+6*c5* ... +(z1-b)^5/m^6-6*d0*(z1-b)^5/m^6)+27660*s(43)+(480*a0*(1-(z1-b)/m)^5/m-480* ... +a1/m*(1-(z1-b)/m)^5+2400*s(26)-2400*s(27)+4800*s(28)-4800*s(29)+4800* ... +s(30)-4800*s(31)+2400*s(32)-2400*s(25)+480*a5*(z1-b)^5/m^6-480*a0* ... +(z1-b)^5/m^6)*cos(s(55))-13830*s(46)+(9600*a0*(1-(z1-b)/m)^5/m-9600*a1/m* ... +(1-(z1-b)/m)^5+48000*s(26)-48000*s(27)+96000*s(28)-96000*s(29)+96000* ... +s(30)-96000*s(31)+48000*s(32)-48000*s(25)+9600*a5*(z1-b)^5/m^6-9600*a0* ... +(z1-b)^5/m^6)*sin(s(55))-(480*a0*(1-(z1-b)/m)^5/m-480*a1/m* ... +(1-(z1-b)/m)^5+2400*s(26)-2400*s(27)+4800*s(28)-4800*s(29)+4800*s(30)-4800* ... +s(31)+2400*s(32)-2400*s(25)+480*a5*(z1-b)^5/m^6-480*a0*(z1-b)^5/m^6)* ... +cos(s(57))-(9600*a0*(1-(z1-b)/m)^5/m-9600*a1/m*(1-(z1-b)/m)^5+48000* ... +s(26)-48000*s(27)+96000*s(28)-96000*s(29)+96000*s(30)-96000*s(31)+48000* ... +s(32)-48000*s(25)+9600*a5*(z1-b)^5/m^6-9600*a0*(z1-b)^5/m^6)* ... +sin(s(57))+12930*s(7)+(492*d0*(1-(z1-b)/m)^5/m-492*d1/m* ... +(1-(z1-b)/m)^5+2460*s(49)-2460*s(38)+4920*s(37)-4920*s(36)+4920*s(48)-4920* ... +s(35)+2460*s(34)-2460*s(33)+492*d5*(z1-b)^5/m^6-492*c0*(z1-b)^5/m^6)* ... +s(58)-(492*d0*(1-(z1-b)/m)^5/m-492*d1/m*(1-(z1-b)/m)^5+2460*s(49)-2460* ... +s(38)+4920*s(37)-4920*s(36)+4920*s(48)-4920*s(35)+2460*s(34)-2460* ... +s(33)+492*d5*(z1-b)^5/m^6-492*c0*(z1-b)^5/m^6)*s(56)-9930*s(38)-478*s(47)* ... +(6*d0*(1-(z1-b)/m)^5/m-6*d1/m*(1-(z1-b)/m)^5+30*s(49)-30*s(38)+60*s(37)-60* ... +s(36)+60*s(48)-60*s(35)+30*s(34)-30*s(33)+6*d5*(z1-b)^5/m^6-6*c0* ... +(z1-b)^5/m^6)); + +s(1) = b5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(2) = b4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(3) = b4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(4) = b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(5) = b3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(6) = b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(7) = b2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(8) = b1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(9) = c5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(10) = c4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(11) = c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(12) = c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(13) = c1*(z1-b)/m*(1-(z1-b)/m)^5; +s(14) = d5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(15) = d4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(16) = d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(17) = d2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(18) = d1*(z1-b)/m*(1-(z1-b)/m)^5; +s(19) = b5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(20) = b4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(21) = b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(22) = b2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(23) = b1*(z1-b)/m*(1-(z1-b)/m)^5; +s(24) = cos(-1/2*b0*(1-(z1-b)/m)^6-3*b1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*b2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*b0* ... +(z1-b)^6/m^6+1/2*d0*(1-(z1-b)/m)^6+3*d1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*c0* ... +(z1-b)^6/m^6+2*z1+1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6); +s(25) = a5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(26) = a1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(27) = a2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(28) = a2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(29) = a3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(30) = a3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(31) = a4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(32) = a4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(33) = d5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(34) = d4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(35) = d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(36) = d3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(37) = d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(38) = d2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(39) = c5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(40) = c4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(41) = c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(42) = c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(43) = c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(44) = c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(45) = c2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(46) = c1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(47) = cos(1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6-2*z1+1/2*b0*(1-(z1-b)/m)^6+3*b1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +b2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*b0* ... +(z1-b)^6/m^6-1/2*d0*(1-(z1-b)/m)^6-3*d1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*c0* ... +(z1-b)^6/m^6); +s(48) = d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(49) = d1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(50) = a5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(51) = a4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(52) = a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(53) = a2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(54) = a1*(z1-b)/m*(1-(z1-b)/m)^5; +s(55) = -a0*(1-(z1-b)/m)^6-6*a1*(z1-b)/m*(1-(z1-b)/m)^5-15*a2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-20*a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15*a4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-6*a5*(z1-b)^5/m^5*(1-(z1-b)/m)-a0*(z1-b)^6/m^6+1/2*c0* ... +(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6+z1; +s(56) = cos(1/2*b0*(1-(z1-b)/m)^6+3*b1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*b2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*b0* ... +(z1-b)^6/m^6+1/2*d0*(1-(z1-b)/m)^6+3*d1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*c0* ... +(z1-b)^6/m^6-2*z1+1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6); +s(57) = -a0*(1-(z1-b)/m)^6-6*a1*(z1-b)/m*(1-(z1-b)/m)^5-15*a2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-20*a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15*a4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-6*a5*(z1-b)^5/m^5*(1-(z1-b)/m)-a0*(z1-b)^6/m^6-1/2*c0* ... +(1-(z1-b)/m)^6-3*c1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*d0*(z1-b)^6/m^6+z1; +s(58) = cos(-1/2*b0*(1-(z1-b)/m)^6-3*b1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*b2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*b4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*b0* ... +(z1-b)^6/m^6-1/2*d0*(1-(z1-b)/m)^6-3*d1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*d2* ... +(z1-b)^2/m^2*(1-(z1-b)/m)^4-10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*d4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2-3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*c0* ... +(z1-b)^6/m^6+2*z1+1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2* ... +c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4* ... +(z1-b)^4/m^4*(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0* ... +(z1-b)^6/m^6); + +% beta +beta = -981/500*cos(a0*(1-(z1-b)/m)^6+6*s(54)+15*s(53)+20*s(52)+15*s(51)+6* ... +s(50)+a0*(z1-b)^6/m^6)+1345255995021145/8796093022208*cos(1/2*c0* ... +(1-(z1-b)/m)^6+3*s(13)+15/2*s(12)+10*s(11)+15/2*s(10)+3*s(9)+1/2*d0* ... +(z1-b)^6/m^6+z1)-4697609773505525/35184372088832*cos(1/2*c0* ... +(1-(z1-b)/m)^6+3*s(13)+15/2*s(12)+10*s(11)+15/2*s(10)+3*s(9)+1/2*d0* ... +(z1-b)^6/m^6-z1)+3299717078230185/140737488355328*cos(-1/2*b0* ... +(1-(z1-b)/m)^6-3*s(23)-15/2*s(22)-10*s(21)-15/2*s(20)-3*s(19)-1/2*b0* ... +(z1-b)^6/m^6+1/2*d0*(1-(z1-b)/m)^6+3*s(18)+15/2*s(17)+10*s(16)+15/2* ... +s(15)+3*s(14)+1/2*c0*(z1-b)^6/m^6+z1)-40221/10000*cos(1/2*b0* ... +(1-(z1-b)/m)^6+3*s(23)+15/2*s(22)+10*s(21)+15/2*s(20)+3*s(19)+1/2*b0* ... +(z1-b)^6/m^6+1/2*d0*(1-(z1-b)/m)^6+3*s(18)+15/2*s(17)+10*s(16)+15/2* ... +s(15)+3*s(14)+1/2*c0*(z1-b)^6/m^6-z1)+981/25*sin(a0*(1-(z1-b)/m)^6+6* ... +s(54)+15*s(53)+20*s(52)+15*s(51)+6*s(50)+a0*(z1-b)^6/m^6); + +s(1) = b2*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(2) = a5*(z1-b)^3/m^5*(1-(z1-b)/m); +s(3) = a4*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(4) = a3*(z1-b)^3/m^5*(1-(z1-b)/m); +s(5) = a3*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(6) = a3*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(7) = a2*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(8) = a2*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(9) = a1*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(10) = d5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(11) = d4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(12) = d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(13) = d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(14) = d3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(15) = d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(16) = d2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(17) = d1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(18) = 6*d0*(1-(z1-b)/m)^5/m-6*d1/m*(1-(z1-b)/m)^5+30*d1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*d2*(z1-b)/m^2*(1-(z1-b)/m)^4+60*d2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-60*d3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+60*d3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-60*d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*d4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-30*d5*(z1-b)^4/m^5*(1-(z1-b)/m)+6*d5*(z1-b)^5/m^6-6*c0* ... +(z1-b)^5/m^6; +s(19) = b5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(20) = b4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(21) = b4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(22) = b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(23) = b3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(24) = b2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(25) = b1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(26) = b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(27) = c1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(28) = c2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(29) = c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(30) = c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(31) = c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(32) = c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(33) = c4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(34) = c5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(35) = -2-3*b5*(z1-b)^5/m^6-3*d1/m*(1-(z1-b)/m)^5+3*b0*(z1-b)^5/m^6-3*c0* ... +(1-(z1-b)/m)^5/m+3*b1/m*(1-(z1-b)/m)^5-3*b0*(1-(z1-b)/m)^5/m+3*d5* ... +(z1-b)^5/m^6-3*c5*(z1-b)^5/m^6-3*c0*(z1-b)^5/m^6+3*d0*(1-(z1-b)/m)^5/m+3* ... +d0*(z1-b)^5/m^6+3*c1/m*(1-(z1-b)/m)^5+30*d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+15* ... +c5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*c4*(z1-b)^4/m^5*(1-(z1-b)/m)+30*c4* ... +(z1-b)^3/m^4*(1-(z1-b)/m)^2-30*c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*c3* ... +(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3+15*c2* ... +(z1-b)/m^2*(1-(z1-b)/m)^4-15*c1*(z1-b)/m^2*(1-(z1-b)/m)^4+15*d1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-15*d2*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+30*d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*d3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-30*d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+15*d4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-15*d5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*b1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+15*b2*(z1-b)/m^2*(1-(z1-b)/m)^4+30*b3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-30*b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*b4*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-15*b4*(z1-b)^4/m^5*(1-(z1-b)/m)+15*b5*(z1-b)^4/m^5* ... +(1-(z1-b)/m); +s(36) = d5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(37) = d4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(38) = d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(39) = d2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(40) = d1*(z1-b)/m*(1-(z1-b)/m)^5; +s(41) = b5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(42) = b4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(43) = b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(44) = b2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(45) = b1*(z1-b)/m*(1-(z1-b)/m)^5; +s(46) = c5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(47) = c4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(48) = c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(49) = c2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(50) = c1*(z1-b)/m*(1-(z1-b)/m)^5; +s(51) = 1/2*c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6-2*z1+1/2* ... +b0*(1-(z1-b)/m)^6+3*b1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*b2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*b4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*b0*(z1-b)^6/m^6-1/2*d0* ... +(1-(z1-b)/m)^6-3*d1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*d2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*d4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*c0*(z1-b)^6/m^6; +s(52) = b5*(z1-b)^3/m^5*(1-(z1-b)/m); +s(53) = c5*(z1-b)^3/m^5*(1-(z1-b)/m); +s(54) = c4*(z1-b)^3/m^5*(1-(z1-b)/m); +s(55) = c4*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(56) = c3*(z1-b)^3/m^5*(1-(z1-b)/m); +s(57) = c3*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(58) = c3*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(59) = c2*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(60) = c2*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(61) = c1*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(62) = d1*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(63) = b2*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(64) = a4*(z1-b)^3/m^5*(1-(z1-b)/m); +s(65) = b4*(z1-b)^3/m^5*(1-(z1-b)/m); +s(66) = b4*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(67) = b3*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(68) = d4*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(69) = d3*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(70) = d5*(z1-b)^3/m^5*(1-(z1-b)/m); +s(71) = b3*(z1-b)^3/m^5*(1-(z1-b)/m); +s(72) = b3*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(73) = b1*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(74) = a5*(z1-b)^4/m^5*(1-(z1-b)/m); +s(75) = a1*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(76) = a2*(z1-b)/m^2*(1-(z1-b)/m)^4; +s(77) = a2*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(78) = a3*(z1-b)^2/m^3*(1-(z1-b)/m)^3; +s(79) = a3*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(80) = a4*(z1-b)^3/m^4*(1-(z1-b)/m)^2; +s(81) = a4*(z1-b)^4/m^5*(1-(z1-b)/m); +s(82) = 1-6*a1/m*(1-(z1-b)/m)^5-3*c0*(1-(z1-b)/m)^5/m-3*c5*(z1-b)^5/m^6+6*a0* ... +(1-(z1-b)/m)^5/m+3*d0*(z1-b)^5/m^6-6*a0*(z1-b)^5/m^6+6*a5*(z1-b)^5/m^6+3* ... +c1/m*(1-(z1-b)/m)^5+15*c5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*c4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)+30*c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2-30*c3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2+30*c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*c2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3+15*c2*(z1-b)/m^2*(1-(z1-b)/m)^4-15*c1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+30*a4*(z1-b)^4/m^5*(1-(z1-b)/m)-60*a4*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2+60*a3*(z1-b)^3/m^4*(1-(z1-b)/m)^2-60*a3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3+60*a2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*a2*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+30*a1*(z1-b)/m^2*(1-(z1-b)/m)^4-30*a5*(z1-b)^4/m^5* ... +(1-(z1-b)/m); +s(83) = a5*(z1-b)^5/m^5*(1-(z1-b)/m); +s(84) = a4*(z1-b)^4/m^4*(1-(z1-b)/m)^2; +s(85) = a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3; +s(86) = a2*(z1-b)^2/m^2*(1-(z1-b)/m)^4; +s(87) = a1*(z1-b)/m*(1-(z1-b)/m)^5; +s(88) = -a0*(1-(z1-b)/m)^6-6*a1*(z1-b)/m*(1-(z1-b)/m)^5-15*a2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-20*a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15*a4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-6*a5*(z1-b)^5/m^5*(1-(z1-b)/m)-a0*(z1-b)^6/m^6+1/2*c0* ... +(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6+z1; +s(89) = 1-6*a1/m*(1-(z1-b)/m)^5+3*c0*(1-(z1-b)/m)^5/m+3*c5*(z1-b)^5/m^6+6*a0* ... +(1-(z1-b)/m)^5/m-3*d0*(z1-b)^5/m^6-6*a0*(z1-b)^5/m^6+6*a5*(z1-b)^5/m^6-3* ... +c1/m*(1-(z1-b)/m)^5-15*c5*(z1-b)^4/m^5*(1-(z1-b)/m)+15*c4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-30*c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*c3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-30*c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+30*c2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-15*c2*(z1-b)/m^2*(1-(z1-b)/m)^4+15*c1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+30*a4*(z1-b)^4/m^5*(1-(z1-b)/m)-60*a4*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2+60*a3*(z1-b)^3/m^4*(1-(z1-b)/m)^2-60*a3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3+60*a2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*a2*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+30*a1*(z1-b)/m^2*(1-(z1-b)/m)^4-30*a5*(z1-b)^4/m^5* ... +(1-(z1-b)/m); +s(90) = -a0*(1-(z1-b)/m)^6-6*a1*(z1-b)/m*(1-(z1-b)/m)^5-15*a2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-20*a3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15*a4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-6*a5*(z1-b)^5/m^5*(1-(z1-b)/m)-a0*(z1-b)^6/m^6-1/2*c0* ... +(1-(z1-b)/m)^6-3*c1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*d0*(z1-b)^6/m^6+z1; +s(91) = d0*(1-(z1-b)/m)^6+6*d1*(z1-b)/m*(1-(z1-b)/m)^5+15*d2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+20*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15*d4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+6*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+c0*(z1-b)^6/m^6; +s(92) = d4*(z1-b)^3/m^5*(1-(z1-b)/m); +s(93) = d3*(z1-b)^3/m^5*(1-(z1-b)/m); +s(94) = d3*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(95) = d2*(z1-b)^2/m^4*(1-(z1-b)/m)^2; +s(96) = d2*(z1-b)/m^3*(1-(z1-b)/m)^3; +s(97) = 6*c0*(1-(z1-b)/m)^5/m-6*c1/m*(1-(z1-b)/m)^5+30*c1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*c2*(z1-b)/m^2*(1-(z1-b)/m)^4+60*c2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-60*c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+60*c3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-60*c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*c4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-30*c5*(z1-b)^4/m^5*(1-(z1-b)/m)+6*c5*(z1-b)^5/m^6-6*d0* ... +(z1-b)^5/m^6; +s(98) = 6*b0*(1-(z1-b)/m)^5/m-6*b1/m*(1-(z1-b)/m)^5+30*b1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*b2*(z1-b)/m^2*(1-(z1-b)/m)^4+60*b2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-60*b3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+60*b3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-60*b4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*b4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-30*b5*(z1-b)^4/m^5*(1-(z1-b)/m)+6*b5*(z1-b)^5/m^6-6*b0* ... +(z1-b)^5/m^6; +s(99) = 4800*c0*(1-(z1-b)/m)^5/m-4800*c1/m*(1-(z1-b)/m)^5+24000*c1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-24000*c2*(z1-b)/m^2*(1-(z1-b)/m)^4+48000*c2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-48000*c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+48000*c3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-48000*c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+24000*c4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-24000*c5*(z1-b)^4/m^5*(1-(z1-b)/m)+4800*c5*(z1-b)^5/m^6-4800* ... +d0*(z1-b)^5/m^6; +s(100) = -2-3*b5*(z1-b)^5/m^6+3*d1/m*(1-(z1-b)/m)^5+3*b0*(z1-b)^5/m^6-3*c0* ... +(1-(z1-b)/m)^5/m+3*b1/m*(1-(z1-b)/m)^5-3*b0*(1-(z1-b)/m)^5/m-3*d5* ... +(z1-b)^5/m^6-3*c5*(z1-b)^5/m^6+3*c0*(z1-b)^5/m^6-3*d0*(1-(z1-b)/m)^5/m+3* ... +d0*(z1-b)^5/m^6+3*c1/m*(1-(z1-b)/m)^5-30*d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+15* ... +c5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*c4*(z1-b)^4/m^5*(1-(z1-b)/m)+30*c4* ... +(z1-b)^3/m^4*(1-(z1-b)/m)^2-30*c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*c3* ... +(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3+15*c2* ... +(z1-b)/m^2*(1-(z1-b)/m)^4-15*c1*(z1-b)/m^2*(1-(z1-b)/m)^4-15*d1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3+15*d2*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3+30*d3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3+30*d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2-15*d4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)+15*d5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*b1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+15*b2*(z1-b)/m^2*(1-(z1-b)/m)^4+30*b3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-30*b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*b4*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-15*b4*(z1-b)^4/m^5*(1-(z1-b)/m)+15*b5*(z1-b)^4/m^5* ... +(1-(z1-b)/m); +s(101) = 1/2*b0*(1-(z1-b)/m)^6+3*b1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*b2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*b4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*b0*(z1-b)^6/m^6+1/2*d0* ... +(1-(z1-b)/m)^6+3*d1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*d2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*d4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*c0*(z1-b)^6/m^6-2*z1+1/2* ... +c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6; +s(102) = 492*c0*(1-(z1-b)/m)^5/m-492*c1/m*(1-(z1-b)/m)^5+2460*c1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-2460*c2*(z1-b)/m^2*(1-(z1-b)/m)^4+4920*c2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-4920*c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+4920*c3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-4920*c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+2460*c4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-2460*c5*(z1-b)^4/m^5*(1-(z1-b)/m)+492*c5*(z1-b)^5/m^6-492*d0* ... +(z1-b)^5/m^6; +s(103) = 480*a0*(1-(z1-b)/m)^5/m-480*a1/m*(1-(z1-b)/m)^5+2400*a1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-2400*a2*(z1-b)/m^2*(1-(z1-b)/m)^4+4800*a2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-4800*a3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+4800*a3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-4800*a4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+2400*a4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-2400*a5*(z1-b)^4/m^5*(1-(z1-b)/m)+480*a5*(z1-b)^5/m^6-480*a0* ... +(z1-b)^5/m^6; +s(104) = 9600*a0*(1-(z1-b)/m)^5/m-9600*a1/m*(1-(z1-b)/m)^5+48000*a1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-48000*a2*(z1-b)/m^2*(1-(z1-b)/m)^4+96000*a2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-96000*a3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+96000*a3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-96000*a4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+48000*a4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-48000*a5*(z1-b)^4/m^5*(1-(z1-b)/m)+9600*a5*(z1-b)^5/m^6-9600* ... +a0*(z1-b)^5/m^6; +s(105) = 492*d0*(1-(z1-b)/m)^5/m-492*d1/m*(1-(z1-b)/m)^5+2460*d1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-2460*d2*(z1-b)/m^2*(1-(z1-b)/m)^4+4920*d2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-4920*d3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+4920*d3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-4920*d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+2460*d4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-2460*d5*(z1-b)^4/m^5*(1-(z1-b)/m)+492*d5*(z1-b)^5/m^6-492*c0* ... +(z1-b)^5/m^6; +s(106) = 492*b0*(1-(z1-b)/m)^5/m-492*b1/m*(1-(z1-b)/m)^5+2460*b1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-2460*b2*(z1-b)/m^2*(1-(z1-b)/m)^4+4920*b2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-4920*b3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+4920*b3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-4920*b4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+2460*b4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-2460*b5*(z1-b)^4/m^5*(1-(z1-b)/m)+492*b5*(z1-b)^5/m^6-492*b0* ... +(z1-b)^5/m^6; +s(107) = 240*c0*(1-(z1-b)/m)^5/m-240*c1/m*(1-(z1-b)/m)^5+1200*c1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-1200*c2*(z1-b)/m^2*(1-(z1-b)/m)^4+2400*c2*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-2400*c3*(z1-b)^2/m^3*(1-(z1-b)/m)^3+2400*c3*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-2400*c4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+1200*c4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-1200*c5*(z1-b)^4/m^5*(1-(z1-b)/m)+240*c5*(z1-b)^5/m^6-240*d0* ... +(z1-b)^5/m^6; +s(108) = -1/2*b0*(1-(z1-b)/m)^6-3*b1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*b2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*b4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*b0*(z1-b)^6/m^6+1/2*d0* ... +(1-(z1-b)/m)^6+3*d1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*d2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*d4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*c0*(z1-b)^6/m^6+2*z1+1/2* ... +c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6; +s(109) = -2-3*b5*(z1-b)^5/m^6+3*d1/m*(1-(z1-b)/m)^5+3*b0*(z1-b)^5/m^6+3*c0* ... +(1-(z1-b)/m)^5/m+3*b1/m*(1-(z1-b)/m)^5-3*b0*(1-(z1-b)/m)^5/m-3*d5* ... +(z1-b)^5/m^6+3*c5*(z1-b)^5/m^6+3*c0*(z1-b)^5/m^6-3*d0*(1-(z1-b)/m)^5/m-3* ... +d0*(z1-b)^5/m^6-3*c1/m*(1-(z1-b)/m)^5-30*d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2-15* ... +c5*(z1-b)^4/m^5*(1-(z1-b)/m)+15*c4*(z1-b)^4/m^5*(1-(z1-b)/m)-30*c4* ... +(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2-30*c3* ... +(z1-b)^2/m^3*(1-(z1-b)/m)^3+30*c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-15*c2* ... +(z1-b)/m^2*(1-(z1-b)/m)^4+15*c1*(z1-b)/m^2*(1-(z1-b)/m)^4-15*d1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3+15*d2*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3+30*d3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3+30*d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2-15*d4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)+15*d5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*b1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+15*b2*(z1-b)/m^2*(1-(z1-b)/m)^4+30*b3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-30*b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*b4*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-15*b4*(z1-b)^4/m^5*(1-(z1-b)/m)+15*b5*(z1-b)^4/m^5* ... +(1-(z1-b)/m); +s(110) = -1/2*b0*(1-(z1-b)/m)^6-3*b1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*b2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*b3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*b4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*b5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*b0*(z1-b)^6/m^6-1/2*d0* ... +(1-(z1-b)/m)^6-3*d1*(z1-b)/m*(1-(z1-b)/m)^5-15/2*d2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4-10*d3*(z1-b)^3/m^3*(1-(z1-b)/m)^3-15/2*d4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2-3*d5*(z1-b)^5/m^5*(1-(z1-b)/m)-1/2*c0*(z1-b)^6/m^6+2*z1+1/2* ... +c0*(1-(z1-b)/m)^6+3*c1*(z1-b)/m*(1-(z1-b)/m)^5+15/2*c2*(z1-b)^2/m^2* ... +(1-(z1-b)/m)^4+10*c3*(z1-b)^3/m^3*(1-(z1-b)/m)^3+15/2*c4*(z1-b)^4/m^4* ... +(1-(z1-b)/m)^2+3*c5*(z1-b)^5/m^5*(1-(z1-b)/m)+1/2*d0*(z1-b)^6/m^6; +s(111) = -2-3*b5*(z1-b)^5/m^6-3*d1/m*(1-(z1-b)/m)^5+3*b0*(z1-b)^5/m^6+3*c0* ... +(1-(z1-b)/m)^5/m+3*b1/m*(1-(z1-b)/m)^5-3*b0*(1-(z1-b)/m)^5/m+3*d5* ... +(z1-b)^5/m^6+3*c5*(z1-b)^5/m^6-3*c0*(z1-b)^5/m^6+3*d0*(1-(z1-b)/m)^5/m-3* ... +d0*(z1-b)^5/m^6-3*c1/m*(1-(z1-b)/m)^5+30*d3*(z1-b)^3/m^4*(1-(z1-b)/m)^2-15* ... +c5*(z1-b)^4/m^5*(1-(z1-b)/m)+15*c4*(z1-b)^4/m^5*(1-(z1-b)/m)-30*c4* ... +(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*c3*(z1-b)^3/m^4*(1-(z1-b)/m)^2-30*c3* ... +(z1-b)^2/m^3*(1-(z1-b)/m)^3+30*c2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-15*c2* ... +(z1-b)/m^2*(1-(z1-b)/m)^4+15*c1*(z1-b)/m^2*(1-(z1-b)/m)^4+15*d1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4-30*b2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-15*d2*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+30*d2*(z1-b)^2/m^3*(1-(z1-b)/m)^3-30*d3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-30*d4*(z1-b)^3/m^4*(1-(z1-b)/m)^2+15*d4*(z1-b)^4/m^5* ... +(1-(z1-b)/m)-15*d5*(z1-b)^4/m^5*(1-(z1-b)/m)-15*b1*(z1-b)/m^2* ... +(1-(z1-b)/m)^4+15*b2*(z1-b)/m^2*(1-(z1-b)/m)^4+30*b3*(z1-b)^2/m^3* ... +(1-(z1-b)/m)^3-30*b3*(z1-b)^3/m^4*(1-(z1-b)/m)^2+30*b4*(z1-b)^3/m^4* ... +(1-(z1-b)/m)^2-15*b4*(z1-b)^4/m^5*(1-(z1-b)/m)+15*b5*(z1-b)^4/m^5* ... +(1-(z1-b)/m); + +jac_alpha = -1000/(-2586*b5*(z1-b)^5/m^6-1986*d1/m*(1-(z1-b)/m)^5+2586*b0* ... +(z1-b)^5/m^6+13320*a1/m*(1-(z1-b)/m)^5-2766*c0*(1-(z1-b)/m)^5/m+2586*b1/m* ... +(1-(z1-b)/m)^5-2586*b0*(1-(z1-b)/m)^5/m+1986*d5*(z1-b)^5/m^6-2766*c5* ... +(z1-b)^5/m^6-25860*s(22)-13320*a0*(1-(z1-b)/m)^5/m+27660*s(30)-1986*c0* ... +(z1-b)^5/m^6+1986*d0*(1-(z1-b)/m)^5/m+2766*d0*(z1-b)^5/m^6-19860* ... +s(12)+133200*s(78)-66600*s(81)+13320*a0*(z1-b)^5/m^6-13320*a5* ... +(z1-b)^5/m^6+2766*c1/m*(1-(z1-b)/m)^5-66600*s(75)-133200*s(79)+133200* ... +s(80)+10560-12930*s(25)+13830*s(34)+9930*s(17)-13830*s(27)+12930* ... +s(24)-12930*s(20)+12930*s(19)-19860*s(14)+27660*s(32)+1600* ... +sin(s(90))+13830*s(28)+9930*s(11)+80*cos(s(90))-9930*s(16)+(984*b0* ... +(1-(z1-b)/m)^5/m-984*b1/m*(1-(z1-b)/m)^5+4920*s(25)-4920*s(24)+9840* ... +s(26)-9840*s(23)+9840*s(22)-9840*s(21)+4920*s(20)-4920*s(19)+984*b5* ... +(z1-b)^5/m^6-984*b0*(z1-b)^5/m^6)*cos(s(91))+66600*s(76)+19860*s(13)+328* ... +cos(s(91))-27660*s(29)-478*cos(s(51))*s(18)+478*cos(s(108))*s(18)-27660* ... +s(31)-9930*s(10)+s(106)*cos(s(110))+19860*s(15)-133200*s(77)+s(102)* ... +cos(s(110))-s(106)*cos(s(101))-s(105)*cos(s(101))-13830*s(33)+s(102)* ... +cos(s(101))+25860*s(23)-25860*s(26)+s(107)*cos(s(90))+s(99)* ... +sin(s(90))-s(104)*sin(s(90))-s(103)*cos(s(90))+s(105)*cos(s(110))+25860* ... +s(21)-478*cos(s(108))*s(98)-10888*cos(c0*(1-(z1-b)/m)^6+6*s(50)+15* ... +s(49)+20*s(48)+15*s(47)+6*s(46)+d0*(z1-b)^6/m^6)-478*cos(s(108))*s(97)+478* ... +cos(s(51))*s(98)-478*cos(s(51))*s(97)+66600*s(74)-80*cos(s(88))-1600* ... +sin(s(88))+s(99)*sin(s(88))+s(104)*sin(s(88))+s(103)*cos(s(88))+s(107)* ... +cos(s(88)))^2*(-532800*s(8)+12930*b4*(z1-b)^4/m^6-165960*s(57)+266400* ... +s(6)+266400*s(9)+82980*s(55)+79440*s(92)-39720*s(93)+119160*s(94)-59580* ... +s(95)+79440*s(96)-110640*s(60)+77580*s(63)+77580*s(66)-39720*s(69)-155160* ... +s(72)+266400*s(2)-110640*s(54)+266400*s(4)-39720*s(70)+478*cos(s(108))* ... +(-30*d0*(1-(z1-b)/m)^4/m^2+60*d1/m^2*(1-(z1-b)/m)^4-120*s(62)-30*d2/m^2* ... +(1-(z1-b)/m)^4+240*s(96)-180*s(95)-120*s(69)+360*s(94)-120*s(93)-180* ... +s(68)+240*s(92)-30*d4*(z1-b)^4/m^6-120*s(70)+60*d5*(z1-b)^4/m^6-30*c0* ... +(z1-b)^4/m^6)-(-2460*d0*(1-(z1-b)/m)^4/m^2+4920*d1/m^2*(1-(z1-b)/m)^4-9840* ... +s(62)-2460*d2/m^2*(1-(z1-b)/m)^4+19680*s(96)-14760*s(95)-9840*s(69)+29520* ... +s(94)-9840*s(93)-14760*s(68)+19680*s(92)-2460*d4*(z1-b)^4/m^6-9840* ... +s(70)+4920*d5*(z1-b)^4/m^6-2460*c0*(z1-b)^4/m^6)*cos(s(101))+(-2460*d0* ... +(1-(z1-b)/m)^4/m^2+4920*d1/m^2*(1-(z1-b)/m)^4-9840*s(62)-2460*d2/m^2* ... +(1-(z1-b)/m)^4+19680*s(96)-14760*s(95)-9840*s(69)+29520*s(94)-9840* ... +s(93)-14760*s(68)+19680*s(92)-2460*d4*(z1-b)^4/m^6-9840*s(70)+4920*d5* ... +(z1-b)^4/m^6-2460*c0*(z1-b)^4/m^6)*cos(s(110))-478*sin(s(51))*s(35)* ... +s(98)+478*sin(s(51))*s(35)*s(97)+51720*s(67)-532800*s(64)+55320* ... +s(61)+55320*s(58)+399600*s(7)+55320*s(53)+51720*s(73)-799200*s(5)-103440* ... +s(1)+478*sin(s(51))*s(35)*s(18)+51720*s(52)+10888*sin(c0*(1-(z1-b)/m)^6+6* ... +s(50)+15*s(49)+20*s(48)+15*s(47)+6*s(46)+d0*(z1-b)^6/m^6)*(-6*c0* ... +(1-(z1-b)/m)^5/m+6*c1/m*(1-(z1-b)/m)^5-30*s(27)+30*s(28)-60*s(29)+60* ... +s(30)-60*s(31)+60*s(32)-30*s(33)+30*s(34)-6*c5*(z1-b)^5/m^6+6*d0* ... +(z1-b)^5/m^6)+478*sin(s(108))*s(111)*s(18)+399600*s(3)+s(102)*sin(s(110))* ... +s(109)-s(102)*sin(s(101))*s(100)+478*cos(s(51))*(-30*b0* ... +(1-(z1-b)/m)^4/m^2+60*b1/m^2*(1-(z1-b)/m)^4-120*s(73)-30*b2/m^2* ... +(1-(z1-b)/m)^4+240*s(1)-180*s(63)-120*s(67)+360*s(72)-120*s(71)-180* ... +s(66)+240*s(65)-30*b4*(z1-b)^4/m^6-120*s(52)+60*b5*(z1-b)^4/m^6-30*b0* ... +(z1-b)^4/m^6)+82980*s(59)+(-48000*a0*(1-(z1-b)/m)^4/m^2+96000*a1/m^2* ... +(1-(z1-b)/m)^4-192000*s(9)-48000*a2/m^2*(1-(z1-b)/m)^4+384000*s(8)-288000* ... +s(7)-192000*s(6)+576000*s(5)-192000*s(4)-288000*s(3)+384000*s(64)-48000*a4* ... +(z1-b)^4/m^6-192000*s(2)+96000*a5*(z1-b)^4/m^6-48000*a0*(z1-b)^4/m^6)* ... +sin(s(88))-(-2400*a0*(1-(z1-b)/m)^4/m^2+4800*a1/m^2*(1-(z1-b)/m)^4-9600* ... +s(9)-2400*a2/m^2*(1-(z1-b)/m)^4+19200*s(8)-14400*s(7)-9600*s(6)+28800* ... +s(5)-9600*s(4)-14400*s(3)+19200*s(64)-2400*a4*(z1-b)^4/m^6-9600*s(2)+4800* ... +a5*(z1-b)^4/m^6-2400*a0*(z1-b)^4/m^6)*cos(s(90))+(-2400*a0* ... +(1-(z1-b)/m)^4/m^2+4800*a1/m^2*(1-(z1-b)/m)^4-9600*s(9)-2400*a2/m^2* ... +(1-(z1-b)/m)^4+19200*s(8)-14400*s(7)-9600*s(6)+28800*s(5)-9600*s(4)-14400* ... +s(3)+19200*s(64)-2400*a4*(z1-b)^4/m^6-9600*s(2)+4800*a5*(z1-b)^4/m^6-2400* ... +a0*(z1-b)^4/m^6)*cos(s(88))-(-48000*a0*(1-(z1-b)/m)^4/m^2+96000*a1/m^2* ... +(1-(z1-b)/m)^4-192000*s(9)-48000*a2/m^2*(1-(z1-b)/m)^4+384000*s(8)-288000* ... +s(7)-192000*s(6)+576000*s(5)-192000*s(4)-288000*s(3)+384000*s(64)-48000*a4* ... +(z1-b)^4/m^6-192000*s(2)+96000*a5*(z1-b)^4/m^6-48000*a0*(z1-b)^4/m^6)* ... +sin(s(90))+55320*s(56)-103440*s(65)-39720*s(62)+s(105)*sin(s(101))* ... +s(100)+s(106)*sin(s(101))*s(100)-(-2460*b0*(1-(z1-b)/m)^4/m^2+4920*b1/m^2* ... +(1-(z1-b)/m)^4-9840*s(73)-2460*b2/m^2*(1-(z1-b)/m)^4+19680*s(1)-14760* ... +s(63)-9840*s(67)+29520*s(72)-9840*s(71)-14760*s(66)+19680*s(65)-2460*b4* ... +(z1-b)^4/m^6-9840*s(52)+4920*b5*(z1-b)^4/m^6-2460*b0*(z1-b)^4/m^6)* ... +cos(s(101))+(-2460*b0*(1-(z1-b)/m)^4/m^2+4920*b1/m^2*(1-(z1-b)/m)^4-9840* ... +s(73)-2460*b2/m^2*(1-(z1-b)/m)^4+19680*s(1)-14760*s(63)-9840*s(67)+29520* ... +s(72)-9840*s(71)-14760*s(66)+19680*s(65)-2460*b4*(z1-b)^4/m^6-9840* ... +s(52)+4920*b5*(z1-b)^4/m^6-2460*b0*(z1-b)^4/m^6)*cos(s(110))-478* ... +cos(s(108))*(-30*b0*(1-(z1-b)/m)^4/m^2+60*b1/m^2*(1-(z1-b)/m)^4-120* ... +s(73)-30*b2/m^2*(1-(z1-b)/m)^4+240*s(1)-180*s(63)-120*s(67)+360*s(72)-120* ... +s(71)-180*s(66)+240*s(65)-30*b4*(z1-b)^4/m^6-120*s(52)+60*b5* ... +(z1-b)^4/m^6-30*b0*(z1-b)^4/m^6)+(-4920*b0*(1-(z1-b)/m)^4/m^2+9840*b1/m^2* ... +(1-(z1-b)/m)^4-19680*s(73)-4920*b2/m^2*(1-(z1-b)/m)^4+39360*s(1)-29520* ... +s(63)-19680*s(67)+59040*s(72)-19680*s(71)-29520*s(66)+39360*s(65)-4920*b4* ... +(z1-b)^4/m^6-19680*s(52)+9840*b5*(z1-b)^4/m^6-4920*b0*(z1-b)^4/m^6)* ... +cos(s(91))+s(106)*sin(s(110))*s(109)-59580*s(68)-80*sin(s(90))*s(89)+1600* ... +cos(s(90))*s(89)+s(105)*sin(s(110))*s(109)-(984*b0*(1-(z1-b)/m)^5/m-984* ... +b1/m*(1-(z1-b)/m)^5+4920*s(25)-4920*s(24)+9840*s(26)-9840*s(23)+9840* ... +s(22)-9840*s(21)+4920*s(20)-4920*s(19)+984*b5*(z1-b)^5/m^6-984*b0* ... +(z1-b)^5/m^6)*sin(s(91))*(-6*d0*(1-(z1-b)/m)^5/m+6*d1/m*(1-(z1-b)/m)^5-30* ... +s(17)+30*s(16)-60*s(15)+60*s(14)-60*s(13)+60*s(12)-30*s(11)+30*s(10)-6*d5* ... +(z1-b)^5/m^6+6*c0*(z1-b)^5/m^6)-328*sin(s(91))*(-6*d0*(1-(z1-b)/m)^5/m+6* ... +d1/m*(1-(z1-b)/m)^5-30*s(17)+30*s(16)-60*s(15)+60*s(14)-60*s(13)+60* ... +s(12)-30*s(11)+30*s(10)-6*d5*(z1-b)^5/m^6+6*c0*(z1-b)^5/m^6)+s(99)* ... +cos(s(90))*s(89)+s(103)*sin(s(90))*s(89)-s(107)*sin(s(90))*s(89)-s(104)* ... +cos(s(90))*s(89)+51720*s(71)-478*cos(s(51))*(-30*c0*(1-(z1-b)/m)^4/m^2+60* ... +c1/m^2*(1-(z1-b)/m)^4-120*s(61)-30*c2/m^2*(1-(z1-b)/m)^4+240*s(60)-180* ... +s(59)-120*s(58)+360*s(57)-120*s(56)-180*s(55)+240*s(54)-30*c4* ... +(z1-b)^4/m^6-120*s(53)+60*c5*(z1-b)^4/m^6-30*d0*(z1-b)^4/m^6)-478* ... +cos(s(51))*(-30*d0*(1-(z1-b)/m)^4/m^2+60*d1/m^2*(1-(z1-b)/m)^4-120* ... +s(62)-30*d2/m^2*(1-(z1-b)/m)^4+240*s(96)-180*s(95)-120*s(69)+360*s(94)-120* ... +s(93)-180*s(68)+240*s(92)-30*d4*(z1-b)^4/m^6-120*s(70)+60*d5* ... +(z1-b)^4/m^6-30*c0*(z1-b)^4/m^6)-478*sin(s(108))*s(111)*s(97)+(-24000*c0* ... +(1-(z1-b)/m)^4/m^2+48000*c1/m^2*(1-(z1-b)/m)^4-96000*s(61)-24000*c2/m^2* ... +(1-(z1-b)/m)^4+192000*s(60)-144000*s(59)-96000*s(58)+288000*s(57)-96000* ... +s(56)-144000*s(55)+192000*s(54)-24000*c4*(z1-b)^4/m^6-96000*s(53)+48000*c5* ... +(z1-b)^4/m^6-24000*d0*(z1-b)^4/m^6)*sin(s(90))+(-1200*c0* ... +(1-(z1-b)/m)^4/m^2+2400*c1/m^2*(1-(z1-b)/m)^4-4800*s(61)-1200*c2/m^2* ... +(1-(z1-b)/m)^4+9600*s(60)-7200*s(59)-4800*s(58)+14400*s(57)-4800* ... +s(56)-7200*s(55)+9600*s(54)-1200*c4*(z1-b)^4/m^6-4800*s(53)+2400*c5* ... +(z1-b)^4/m^6-1200*d0*(z1-b)^4/m^6)*cos(s(88))+(-24000*c0* ... +(1-(z1-b)/m)^4/m^2+48000*c1/m^2*(1-(z1-b)/m)^4-96000*s(61)-24000*c2/m^2* ... +(1-(z1-b)/m)^4+192000*s(60)-144000*s(59)-96000*s(58)+288000*s(57)-96000* ... +s(56)-144000*s(55)+192000*s(54)-24000*c4*(z1-b)^4/m^6-96000*s(53)+48000*c5* ... +(z1-b)^4/m^6-24000*d0*(z1-b)^4/m^6)*sin(s(88))+(-2460*c0* ... +(1-(z1-b)/m)^4/m^2+4920*c1/m^2*(1-(z1-b)/m)^4-9840*s(61)-2460*c2/m^2* ... +(1-(z1-b)/m)^4+19680*s(60)-14760*s(59)-9840*s(58)+29520*s(57)-9840* ... +s(56)-14760*s(55)+19680*s(54)-2460*c4*(z1-b)^4/m^6-9840*s(53)+4920*c5* ... +(z1-b)^4/m^6-2460*d0*(z1-b)^4/m^6)*cos(s(110))-478*cos(s(108))*(-30*c0* ... +(1-(z1-b)/m)^4/m^2+60*c1/m^2*(1-(z1-b)/m)^4-120*s(61)-30*c2/m^2* ... +(1-(z1-b)/m)^4+240*s(60)-180*s(59)-120*s(58)+360*s(57)-120*s(56)-180* ... +s(55)+240*s(54)-30*c4*(z1-b)^4/m^6-120*s(53)+60*c5*(z1-b)^4/m^6-30*d0* ... +(z1-b)^4/m^6)+(-1200*c0*(1-(z1-b)/m)^4/m^2+2400*c1/m^2*(1-(z1-b)/m)^4-4800* ... +s(61)-1200*c2/m^2*(1-(z1-b)/m)^4+9600*s(60)-7200*s(59)-4800*s(58)+14400* ... +s(57)-4800*s(56)-7200*s(55)+9600*s(54)-1200*c4*(z1-b)^4/m^6-4800* ... +s(53)+2400*c5*(z1-b)^4/m^6-1200*d0*(z1-b)^4/m^6)*cos(s(90))+(-2460*c0* ... +(1-(z1-b)/m)^4/m^2+4920*c1/m^2*(1-(z1-b)/m)^4-9840*s(61)-2460*c2/m^2* ... +(1-(z1-b)/m)^4+19680*s(60)-14760*s(59)-9840*s(58)+29520*s(57)-9840* ... +s(56)-14760*s(55)+19680*s(54)-2460*c4*(z1-b)^4/m^6-9840*s(53)+4920*c5* ... +(z1-b)^4/m^6-2460*d0*(z1-b)^4/m^6)*cos(s(101))-478*sin(s(108))*s(111)* ... +s(98)+13830*c0*(1-(z1-b)/m)^4/m^2+19860*d1/m^2*(1-(z1-b)/m)^4-27660*c1/m^2* ... +(1-(z1-b)/m)^4+13830*c2/m^2*(1-(z1-b)/m)^4+13830*c4*(z1-b)^4/m^6-27660*c5* ... +(z1-b)^4/m^6+13830*d0*(z1-b)^4/m^6+66600*a4*(z1-b)^4/m^6-133200*a5* ... +(z1-b)^4/m^6+66600*a0*(1-(z1-b)/m)^4/m^2+12930*b2/m^2*(1-(z1-b)/m)^4-9930* ... +c0*(z1-b)^4/m^6+12930*b0*(z1-b)^4/m^6-9930*d0*(1-(z1-b)/m)^4/m^2-9930* ... +d2/m^2*(1-(z1-b)/m)^4-9930*d4*(z1-b)^4/m^6+19860*d5*(z1-b)^4/m^6+s(99)* ... +cos(s(88))*s(82)+s(104)*cos(s(88))*s(82)-s(107)*sin(s(88))*s(82)-s(103)* ... +sin(s(88))*s(82)+80*sin(s(88))*s(82)-1600*cos(s(88))*s(82)+12930*b0* ... +(1-(z1-b)/m)^4/m^2-25860*b1/m^2*(1-(z1-b)/m)^4-25860*b5* ... +(z1-b)^4/m^6-133200*a1/m^2*(1-(z1-b)/m)^4+66600*a2/m^2* ... +(1-(z1-b)/m)^4+66600*a0*(z1-b)^4/m^6); + +switch opt + case 1, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_a0(z1); + case 2, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_a1(z1); + case 3, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_a2(z1); + case 4, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_a3(z1); + case 5, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_a4(z1); + case 6, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_a5(z1); + case 7, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_b0(z1); + case 8, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_b1(z1); + case 9, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_b2(z1); + case 10, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_b3(z1); + case 11, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_b4(z1); + case 12, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_b5(z1); + case 13, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_c0(z1); + case 14, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_c1(z1); + case 15, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_c2(z1); + case 16, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_c3(z1); + case 17, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_c4(z1); + case 18, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_c5(z1); + case 19, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_d0(z1); + case 20, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_d1(z1); + case 21, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_d2(z1); + case 22, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_d3(z1); + case 23, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_d4(z1); + case 24, + [partial_alpha,partial_beta,partial_jac_alpha] = zd_diff_cc_d5(z1); + otherwise + error('OPT parameter out of range'); +end + +% state one +dz(1) = 0; + +% state two +dz(2) = z2^2/alpha^2*partial_alpha*jac_alpha ... + + z2^2/alpha*partial_jac_alpha ... + + partial_alpha*beta ... + + alpha*partial_beta; diff --git a/demos/rabbit/anim/rabbit_model/zd_lift.m b/demos/rabbit/anim/rabbit_model/zd_lift.m new file mode 100644 index 0000000..a708d7d --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_lift.m @@ -0,0 +1,138 @@ +function x = zd_lift(theta1,dtheta1) +%ZD_LIFT Lifts the zero dynamics to the full state of the system. +% +% X = ZD_LIFT(THETA1,DTHETA1) + +%Eric Westervelt +%01-May-2001 05:42:40 + +[a,b,m] = controlParameters; + +a0=a(1); a1=a(2); a2=a(3); a3=a(4); a4=a(5); a5=a(6); a6=a(7); +b0=a(8); b1=a(9); b2=a(10); b3=a(11); b4=a(12); b5=a(13); b6=a(14); +c0=a(15); c1=a(16); c2=a(17); c3=a(18); c4=a(19); c5=a(20); c6=a(21); +d0=a(22); d1=a(23); d2=a(24); d3=a(25); d4=a(26); d5=a(27); d6=a(28); + +q31 = -1./2.*c0.*(1-(theta1-b)./m).^6-3.*c1.*(theta1-b)./m.*(1-(theta1- ... + b)./m).^5-15./2.*c2.*(theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4- ... + 10.*c3.*(theta1-b).^3./m.^3.*(1-(theta1-b)./m).^3-15./2.*c4.* ... + (theta1-b).^4./m.^4.*(1-(theta1-b)./m).^2-3.*c5.*(theta1- ... + b).^5./m.^5.*(1-(theta1-b)./m)-1./2.*c6.*(theta1-b).^6./m.^6+ ... + theta1; + +q32 = b0.*(1-(theta1-b)./m).^6+6.*b1.*(theta1-b)./m.*(1-(theta1- ... + b)./m).^5+15.*b2.*(theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4+20.* ... + b3.*(theta1-b).^3./m.^3.*(1-(theta1-b)./m).^3+15.*b4.*(theta1- ... + b).^4./m.^4.*(1-(theta1-b)./m).^2+6.*b5.*(theta1-b).^5./m.^5.*(1- ... + (theta1-b)./m)+b6.*(theta1-b).^6./m.^6-1./2.*c0.*(1-(theta1- ... + b)./m).^6-3.*c1.*(theta1-b)./m.*(1-(theta1-b)./m).^5-15./2.*c2.* ... + (theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4-10.*c3.*(theta1- ... + b).^3./m.^3.*(1-(theta1-b)./m).^3-15./2.*c4.*(theta1- ... + b).^4./m.^4.*(1-(theta1-b)./m).^2-3.*c5.*(theta1-b).^5./m.^5.*(1- ... + (theta1-b)./m)-1./2.*c6.*(theta1-b).^6./m.^6+theta1; + +q41 = 1./2.*c0.*(1-(theta1-b)./m).^6+3.*c1.*(theta1-b)./m.*(1-(theta1- ... + b)./m).^5+15./2.*c2.*(theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4+ ... + 10.*c3.*(theta1-b).^3./m.^3.*(1-(theta1-b)./m).^3+15./2.*c4.* ... + (theta1-b).^4./m.^4.*(1-(theta1-b)./m).^2+3.*c5.*(theta1- ... + b).^5./m.^5.*(1-(theta1-b)./m)+1./2.*c6.*(theta1-b).^6./m.^6+ ... + theta1; + +q42 = b0.*(1-(theta1-b)./m).^6+6.*b1.*(theta1-b)./m.*(1-(theta1- ... + b)./m).^5+15.*b2.*(theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4+20.* ... + b3.*(theta1-b).^3./m.^3.*(1-(theta1-b)./m).^3+15.*b4.*(theta1- ... + b).^4./m.^4.*(1-(theta1-b)./m).^2+6.*b5.*(theta1-b).^5./m.^5.*(1- ... + (theta1-b)./m)+b6.*(theta1-b).^6./m.^6-1./2.*c0.*(1-(theta1- ... + b)./m).^6-3.*c1.*(theta1-b)./m.*(1-(theta1-b)./m).^5-15./2.*c2.* ... + (theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4-10.*c3.*(theta1- ... + b).^3./m.^3.*(1-(theta1-b)./m).^3-15./2.*c4.*(theta1- ... + b).^4./m.^4.*(1-(theta1-b)./m).^2-3.*c5.*(theta1-b).^5./m.^5.*(1- ... + (theta1-b)./m)-1./2.*c6.*(theta1-b).^6./m.^6+d0.*(1-(theta1- ... + b)./m).^6+6.*d1.*(theta1-b)./m.*(1-(theta1-b)./m).^5+15.*d2.* ... + (theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4+20.*d3.*(theta1- ... + b).^3./m.^3.*(1-(theta1-b)./m).^3+15.*d4.*(theta1-b).^4./m.^4.* ... + (1-(theta1-b)./m).^2+6.*d5.*(theta1-b).^5./m.^5.*(1-(theta1- ... + b)./m)+d6.*(theta1-b).^6./m.^6+theta1; + +q1 = a0.*(1-(theta1-b)./m).^6+6.*a1.*(theta1-b)./m.*(1-(theta1- ... + b)./m).^5+15.*a2.*(theta1-b).^2./m.^2.*(1-(theta1-b)./m).^4+20.* ... + a3.*(theta1-b).^3./m.^3.*(1-(theta1-b)./m).^3+15.*a4.*(theta1- ... + b).^4./m.^4.*(1-(theta1-b)./m).^2+6.*a5.*(theta1-b).^5./m.^5.*(1- ... + (theta1-b)./m)+a6.*(theta1-b).^6./m.^6; + +dq31 = -1./2.*dtheta1.*(-6.*c0.*(1-(theta1-b)./m).^5./m+6.*c1./m.*(1- ... + (theta1-b)./m).^5-30.*c1.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4+ ... + 30.*c2.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4-60.*c2.*(theta1- ... + b).^2./m.^3.*(1-(theta1-b)./m).^3+60.*c3.*(theta1-b).^2./m.^3.* ... + (1-(theta1-b)./m).^3-60.*c3.*(theta1-b).^3./m.^4.*(1-(theta1- ... + b)./m).^2+60.*c4.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2-30.* ... + c4.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)+30.*c5.*(theta1- ... + b).^4./m.^5.*(1-(theta1-b)./m)-6.*c5.*(theta1-b).^5./m.^6+6.*c6.* ... + (theta1-b).^5./m.^6)+dtheta1; + +dq32 = dtheta1.*(-6.*b0.*(1-(theta1-b)./m).^5./m+6.*b1./m.*(1-(theta1- ... + b)./m).^5-30.*b1.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4+30.*b2.* ... + (theta1-b)./m.^2.*(1-(theta1-b)./m).^4-60.*b2.*(theta1- ... + b).^2./m.^3.*(1-(theta1-b)./m).^3+60.*b3.*(theta1-b).^2./m.^3.* ... + (1-(theta1-b)./m).^3-60.*b3.*(theta1-b).^3./m.^4.*(1-(theta1- ... + b)./m).^2+60.*b4.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2-30.* ... + b4.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)+30.*b5.*(theta1- ... + b).^4./m.^5.*(1-(theta1-b)./m)-6.*b5.*(theta1-b).^5./m.^6+6.*b6.* ... + (theta1-b).^5./m.^6)-1./2.*dtheta1.*(-6.*c0.*(1-(theta1- ... + b)./m).^5./m+6.*c1./m.*(1-(theta1-b)./m).^5-30.*c1.*(theta1- ... + b)./m.^2.*(1-(theta1-b)./m).^4+30.*c2.*(theta1-b)./m.^2.*(1- ... + (theta1-b)./m).^4-60.*c2.*(theta1-b).^2./m.^3.*(1-(theta1- ... + b)./m).^3+60.*c3.*(theta1-b).^2./m.^3.*(1-(theta1-b)./m).^3-60.* ... + c3.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2+60.*c4.*(theta1- ... + b).^3./m.^4.*(1-(theta1-b)./m).^2-30.*c4.*(theta1-b).^4./m.^5.* ... + (1-(theta1-b)./m)+30.*c5.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)- ... + 6.*c5.*(theta1-b).^5./m.^6+6.*c6.*(theta1-b).^5./m.^6)+dtheta1; + +dq41 = 1./2.*dtheta1.*(-6.*c0.*(1-(theta1-b)./m).^5./m+6.*c1./m.*(1- ... + (theta1-b)./m).^5-30.*c1.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4+ ... + 30.*c2.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4-60.*c2.*(theta1- ... + b).^2./m.^3.*(1-(theta1-b)./m).^3+60.*c3.*(theta1-b).^2./m.^3.* ... + (1-(theta1-b)./m).^3-60.*c3.*(theta1-b).^3./m.^4.*(1-(theta1- ... + b)./m).^2+60.*c4.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2-30.* ... + c4.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)+30.*c5.*(theta1- ... + b).^4./m.^5.*(1-(theta1-b)./m)-6.*c5.*(theta1-b).^5./m.^6+6.*c6.* ... + (theta1-b).^5./m.^6)+dtheta1; + +dq42 = dtheta1.*(-6.*b0.*(1-(theta1-b)./m).^5./m+6.*b1./m.*(1-(theta1- ... + b)./m).^5-30.*b1.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4+30.*b2.* ... + (theta1-b)./m.^2.*(1-(theta1-b)./m).^4-60.*b2.*(theta1- ... + b).^2./m.^3.*(1-(theta1-b)./m).^3+60.*b3.*(theta1-b).^2./m.^3.* ... + (1-(theta1-b)./m).^3-60.*b3.*(theta1-b).^3./m.^4.*(1-(theta1- ... + b)./m).^2+60.*b4.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2-30.* ... + b4.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)+30.*b5.*(theta1- ... + b).^4./m.^5.*(1-(theta1-b)./m)-6.*b5.*(theta1-b).^5./m.^6+6.*b6.* ... + (theta1-b).^5./m.^6)-1./2.*dtheta1.*(-6.*c0.*(1-(theta1- ... + b)./m).^5./m+6.*c1./m.*(1-(theta1-b)./m).^5-30.*c1.*(theta1- ... + b)./m.^2.*(1-(theta1-b)./m).^4+30.*c2.*(theta1-b)./m.^2.*(1- ... + (theta1-b)./m).^4-60.*c2.*(theta1-b).^2./m.^3.*(1-(theta1- ... + b)./m).^3+60.*c3.*(theta1-b).^2./m.^3.*(1-(theta1-b)./m).^3-60.* ... + c3.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2+60.*c4.*(theta1- ... + b).^3./m.^4.*(1-(theta1-b)./m).^2-30.*c4.*(theta1-b).^4./m.^5.* ... + (1-(theta1-b)./m)+30.*c5.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)- ... + 6.*c5.*(theta1-b).^5./m.^6+6.*c6.*(theta1-b).^5./m.^6)+dtheta1.* ... + (-6.*d0.*(1-(theta1-b)./m).^5./m+6.*d1./m.*(1-(theta1-b)./m).^5- ... + 30.*d1.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4+30.*d2.*(theta1- ... + b)./m.^2.*(1-(theta1-b)./m).^4-60.*d2.*(theta1-b).^2./m.^3.*(1- ... + (theta1-b)./m).^3+60.*d3.*(theta1-b).^2./m.^3.*(1-(theta1- ... + b)./m).^3-60.*d3.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2+60.* ... + d4.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2-30.*d4.*(theta1- ... + b).^4./m.^5.*(1-(theta1-b)./m)+30.*d5.*(theta1-b).^4./m.^5.*(1- ... + (theta1-b)./m)-6.*d5.*(theta1-b).^5./m.^6+6.*d6.*(theta1- ... + b).^5./m.^6)+dtheta1; + +dq1 = dtheta1.*(-6.*a0.*(1-(theta1-b)./m).^5./m+6.*a1./m.*(1-(theta1- ... + b)./m).^5-30.*a1.*(theta1-b)./m.^2.*(1-(theta1-b)./m).^4+30.*a2.* ... + (theta1-b)./m.^2.*(1-(theta1-b)./m).^4-60.*a2.*(theta1- ... + b).^2./m.^3.*(1-(theta1-b)./m).^3+60.*a3.*(theta1-b).^2./m.^3.* ... + (1-(theta1-b)./m).^3-60.*a3.*(theta1-b).^3./m.^4.*(1-(theta1- ... + b)./m).^2+60.*a4.*(theta1-b).^3./m.^4.*(1-(theta1-b)./m).^2-30.* ... + a4.*(theta1-b).^4./m.^5.*(1-(theta1-b)./m)+30.*a5.*(theta1- ... + b).^4./m.^5.*(1-(theta1-b)./m)-6.*a5.*(theta1-b).^5./m.^6+6.*a6.* ... + (theta1-b).^5./m.^6); + +x = [q31; q32; q41; q42; q1; dq31; dq32; dq41; dq42; dq1]; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/zd_lift_special.m b/demos/rabbit/anim/rabbit_model/zd_lift_special.m new file mode 100644 index 0000000..d8919c0 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_lift_special.m @@ -0,0 +1,21 @@ +function x = zd_lift_special(z) +%ZD_LIFT_SPECIAL Lifts the zero dynamics to the full state of the +%system. +% +% X = ZD_LIFT_SPECIAL(Z) + +%Eric Westervelt +%3/9/01 + +[n,m] = size(z); + +if n ~= 1 & m ~= 1 + dz = []; + for k = 1:length(z) + dz(k,:) = zd(z(k,:))'; + x(k,:) = zd_lift(z(k,1)',dz(k,1)')'; + end +else + dz = zd(z)'; + x = zd_lift(z(1)',dz(1)')'; +end diff --git a/demos/rabbit/anim/rabbit_model/zd_model.mdl b/demos/rabbit/anim/rabbit_model/zd_model.mdl new file mode 100644 index 0000000..37037ac --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_model.mdl @@ -0,0 +1,842 @@ +Model { + Name "zd_model" + Version 4.00 + SampleTimeColors off + LibraryLinkDisplay "none" + WideLines off + ShowLineDimensions on + ShowPortDataTypes off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovNameIncrementing off + CovHtmlReporting on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + Created "Mon Mar 12 12:11:31 2001" + Creator "ewesterv" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "ewesterv" + ModifiedDateFormat "%" + LastModifiedDate "Mon Mar 12 14:55:17 2001" + ModelVersionFormat "1.%" + ConfigurationManager "none" + SimParamPage "Solver" + StartTime "0.0" + StopTime "T" + SolverMode "SingleTasking" + Solver "ode45" + RelTol "1e-5" + AbsTol "1e-6" + Refine "1" + MaxStep "auto" + MinStep "auto" + MaxNumMinSteps "-1" + InitialStep "auto" + FixedStep ".001" + MaxOrder 5 + OutputOption "RefineOutputTimes" + OutputTimes "[]" + LoadExternalInput off + ExternalInput "[t, u]" + SaveTime on + TimeSaveName "t" + SaveState off + StateSaveName "xout" + SaveOutput on + OutputSaveName "yout" + LoadInitialState off + InitialState "xInitial" + SaveFinalState off + FinalStateName "xFinal" + SaveFormat "Array" + LimitDataPoints off + MaxDataPoints "1000" + Decimation "1" + AlgebraicLoopMsg "warning" + MinStepSizeMsg "warning" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + InheritedTsInSrcMsg "warning" + SingleTaskRateTransMsg "none" + MultiTaskRateTransMsg "error" + IntegerOverflowMsg "warning" + CheckForMatrixSingularity "none" + UnnecessaryDatatypeConvMsg "none" + Int32ToFloatConvMsg "warning" + SignalLabelMismatchMsg "none" + LinearizationMsg "none" + VectorMatrixConversionMsg "none" + SfunCompatibilityCheckMsg "none" + BlockPriorityViolationMsg "warning" + ArrayBoundsChecking "none" + ConsistencyChecking "none" + ZeroCross on + Profile off + SimulationMode "normal" + RTWSystemTargetFile "grt.tlc" + RTWInlineParameters off + RTWRetainRTWFile off + RTWTemplateMakefile "grt_default_tmf" + RTWMakeCommand "make_rtw" + RTWGenerateCodeOnly off + TLCProfiler off + TLCDebug off + TLCCoverage off + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + ExtModeMexFile "ext_comm" + ExtModeBatchMode off + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeLogAll on + OptimizeBlockIOStorage on + BufferReuse on + ParameterPooling on + BlockReductionOpt off + BooleanDataType off + BlockDefaults { + Orientation "right" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + System { + Name "zd_model" + Location [262, 42, 945, 599] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + ZoomFactor "100" + AutoZoom on + ReportName "simulink-default.rpt" + Block { + BlockType Constant + Name "." + Position [250, 266, 290, 294] + Value "z0" + VectorParams1D on + } + Block { + BlockType Clock + Name "Clock" + Position [295, 435, 315, 455] + DisplayTime off + Decimation "10" + } + Block { + BlockType Constant + Name "Constant1" + Position [290, 475, 320, 505] + Value ".01" + VectorParams1D on + } + Block { + BlockType Integrator + Name "Integrator" + Ports [3, 1, 0, 0, 1] + Position [415, 169, 455, 221] + ExternalReset "rising" + InitialConditionSource "external" + InitialCondition "x_0" + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort on + AbsoluteTolerance "auto" + } + Block { + BlockType Logic + Name "Logical\nOperator1" + Ports [2, 1] + Position [535, 282, 565, 313] + Operator "OR" + Inputs "2" + } + Block { + BlockType Logic + Name "Logical\nOperator2" + Ports [2, 1] + Position [395, 407, 425, 438] + Operator "AND" + Inputs "2" + } + Block { + BlockType Logic + Name "Logical\nOperator3" + Ports [2, 1] + Position [465, 357, 495, 388] + Operator "AND" + Inputs "2" + } + Block { + BlockType SubSystem + Name "RIgid Impact_Model" + Ports [1, 2, 1] + Position [55, 373, 165, 417] + ShowPortLabels on + TreatAsAtomicUnit on + RTWSystemCode "Auto" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + System { + Name "RIgid Impact_Model" + Location [124, 54, 556, 318] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + ZoomFactor "100" + AutoZoom on + Block { + BlockType Inport + Name "q_dq_old" + Position [15, 97, 45, 113] + Port "1" + Interpolate on + } + Block { + BlockType EnablePort + Name "Enable" + Ports [] + Position [20, 195, 40, 215] + StatesWhenEnabling "held" + ShowOutputPort off + } + Block { + BlockType Gain + Name "Matrix\nGain" + Position [235, 13, 275, 47] + Gain "[zeros(1,11) 1 0]" + Multiplication "Matrix(K*u)" + SaturateOnIntegerOverflow on + } + Block { + BlockType Gain + Name "Matrix\nGain1" + Position [235, 89, 275, 121] + Gain "eye(10,13)" + Multiplication "Matrix(K*u)" + SaturateOnIntegerOverflow on + } + Block { + BlockType Gain + Name "Matrix\nGain2" + Position [235, 154, 275, 186] + Gain "[zeros(1,10) 1 0 0]" + Multiplication "Matrix(K*u)" + SaturateOnIntegerOverflow on + } + Block { + BlockType ToWorkspace + Name "To Workspace" + Position [310, 15, 370, 45] + VariableName "mu" + MaxDataPoints "inf" + Decimation "1" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType MATLABFcn + Name "impact model" + Position [80, 85, 180, 125] + MATLABFcn "impact_abs_red" + OutputDimensions "13" + OutputSignalType "auto" + Output1D off + } + Block { + BlockType Outport + Name "q_dq_new" + Position [335, 98, 365, 112] + Port "1" + OutputWhenDisabled "held" + InitialOutput "0" + } + Block { + BlockType Outport + Name "flag" + Position [335, 163, 365, 177] + Port "2" + OutputWhenDisabled "held" + InitialOutput "0" + } + Line { + SrcBlock "q_dq_old" + SrcPort 1 + DstBlock "impact model" + DstPort 1 + } + Line { + SrcBlock "Matrix\nGain1" + SrcPort 1 + DstBlock "q_dq_new" + DstPort 1 + } + Line { + SrcBlock "Matrix\nGain2" + SrcPort 1 + DstBlock "flag" + DstPort 1 + } + Line { + SrcBlock "impact model" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Matrix\nGain1" + DstPort 1 + } + Branch { + Points [0, -75] + DstBlock "Matrix\nGain" + DstPort 1 + } + Branch { + Points [0, 65] + DstBlock "Matrix\nGain2" + DstPort 1 + } + } + Line { + SrcBlock "Matrix\nGain" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + } + } + Block { + BlockType RelationalOperator + Name "Relational\nOperator1" + Position [340, 437, 370, 468] + Operator ">" + } + Block { + BlockType SubSystem + Name "Reset Logic" + Ports [1, 3] + Position [215, 153, 345, 207] + ShowPortLabels on + TreatAsAtomicUnit off + RTWSystemCode "Auto" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + System { + Name "Reset Logic" + Location [281, 28, 934, 401] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + ZoomFactor "100" + AutoZoom on + Block { + BlockType Inport + Name "In1" + Position [15, 112, 45, 128] + Port "1" + Interpolate on + } + Block { + BlockType Clock + Name "Clock" + Position [330, 235, 350, 255] + DisplayTime off + Decimation "10" + } + Block { + BlockType Constant + Name "Constant" + Position [215, 210, 245, 240] + Value "0" + VectorParams1D on + } + Block { + BlockType Constant + Name "Constant1" + Position [325, 275, 355, 305] + Value "0" + VectorParams1D on + } + Block { + BlockType Constant + Name "Constant2" + Position [215, 60, 245, 90] + Value "0" + VectorParams1D on + } + Block { + BlockType Logic + Name "Logical\nOperator1" + Ports [2, 1] + Position [445, 232, 475, 263] + Operator "OR" + Inputs "2" + } + Block { + BlockType Logic + Name "Logical\nOperator2" + Ports [2, 1] + Position [340, 117, 370, 148] + Operator "AND" + Inputs "2" + } + Block { + BlockType RelationalOperator + Name "Relational\nOperator" + Position [265, 192, 295, 223] + Operator "<" + } + Block { + BlockType RelationalOperator + Name "Relational\nOperator1" + Position [380, 237, 410, 268] + Operator "<=" + } + Block { + BlockType RelationalOperator + Name "Relational\nOperator2" + Position [270, 37, 300, 68] + Operator "<=" + } + Block { + BlockType MATLABFcn + Name "swing_foot_height" + Position [85, 24, 175, 66] + MATLABFcn "swing_foot_height" + OutputDimensions "1" + OutputSignalType "auto" + Output1D off + } + Block { + BlockType MATLABFcn + Name "swing_foot_velocity" + Position [85, 179, 175, 221] + MATLABFcn "swing_foot_velocity" + OutputDimensions "1" + OutputSignalType "auto" + Output1D off + } + Block { + BlockType Outport + Name "Foot Touch OR Initial Reset" + Position [550, 243, 580, 257] + Port "1" + OutputWhenDisabled "held" + InitialOutput "0" + } + Block { + BlockType Outport + Name "Foot Touches" + Position [480, 128, 510, 142] + Port "2" + OutputWhenDisabled "held" + InitialOutput "0" + } + Block { + BlockType Outport + Name "Initial Reset at Time Zero" + Position [500, 293, 530, 307] + Port "3" + OutputWhenDisabled "held" + InitialOutput "0" + } + Line { + SrcBlock "Constant" + SrcPort 1 + DstBlock "Relational\nOperator" + DstPort 2 + } + Line { + SrcBlock "Logical\nOperator1" + SrcPort 1 + DstBlock "Foot Touch OR Initial Reset" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Relational\nOperator1" + DstPort 1 + } + Line { + SrcBlock "Constant1" + SrcPort 1 + Points [5, 0] + DstBlock "Relational\nOperator1" + DstPort 2 + } + Line { + SrcBlock "Relational\nOperator1" + SrcPort 1 + Points [10, 0] + Branch { + Points [0, 0] + DstBlock "Logical\nOperator1" + DstPort 2 + } + Branch { + Points [0, 45] + DstBlock "Initial Reset at Time Zero" + DstPort 1 + } + } + Line { + SrcBlock "In1" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "swing_foot_height" + DstPort 1 + } + Branch { + DstBlock "swing_foot_velocity" + DstPort 1 + } + } + Line { + SrcBlock "Constant2" + SrcPort 1 + Points [0, -15] + DstBlock "Relational\nOperator2" + DstPort 2 + } + Line { + SrcBlock "swing_foot_height" + SrcPort 1 + DstBlock "Relational\nOperator2" + DstPort 1 + } + Line { + SrcBlock "swing_foot_velocity" + SrcPort 1 + DstBlock "Relational\nOperator" + DstPort 1 + } + Line { + SrcBlock "Relational\nOperator2" + SrcPort 1 + Points [20, 0] + DstBlock "Logical\nOperator2" + DstPort 1 + } + Line { + SrcBlock "Relational\nOperator" + SrcPort 1 + Points [25, 0] + DstBlock "Logical\nOperator2" + DstPort 2 + } + Line { + SrcBlock "Logical\nOperator2" + SrcPort 1 + Points [55, 0] + Branch { + DstBlock "Logical\nOperator1" + DstPort 1 + } + Branch { + DstBlock "Foot Touches" + DstPort 1 + } + } + } + } + Block { + BlockType Stop + Name "Stop Simulation" + Position [605, 282, 640, 318] + } + Block { + BlockType Switch + Name "Switch" + Position [315, 264, 355, 356] + Threshold "0.5" + } + Block { + BlockType ToWorkspace + Name "To Workspace" + Position [495, 180, 555, 210] + VariableName "z" + MaxDataPoints "inf" + Decimation "1" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType ToWorkspace + Name "To Workspace1" + Position [535, 425, 595, 455] + NamePlacement "alternate" + VariableName "foot_touch" + MaxDataPoints "inf" + Decimation "1" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType ToWorkspace + Name "To Workspace3" + Position [535, 360, 595, 390] + NamePlacement "alternate" + VariableName "sim_error" + MaxDataPoints "inf" + Decimation "1" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType MATLABFcn + Name "lift zero dynamics state" + Position [250, 29, 340, 71] + Orientation "left" + MATLABFcn "zd_lift_special" + OutputDimensions "10" + OutputSignalType "auto" + Output1D off + } + Block { + BlockType MATLABFcn + Name "zero dynamics model" + Position [55, 109, 145, 151] + MATLABFcn "zd" + OutputDimensions "2" + OutputSignalType "auto" + Output1D on + } + Block { + BlockType MATLABFcn + Name "zero dynamics project" + Position [200, 319, 290, 361] + MATLABFcn "zd_project" + OutputDimensions "2" + OutputSignalType "auto" + Output1D on + } + Line { + SrcBlock "Relational\nOperator1" + SrcPort 1 + Points [0, -25] + DstBlock "Logical\nOperator2" + DstPort 2 + } + Line { + SrcBlock "RIgid Impact_Model" + SrcPort 2 + Points [140, 0; 0, -25] + DstBlock "Logical\nOperator3" + DstPort 2 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Relational\nOperator1" + DstPort 1 + } + Line { + SrcBlock "Integrator" + SrcPort 1 + Points [0, 0; 10, 0] + Branch { + DstBlock "To Workspace" + DstPort 1 + } + Branch { + Points [0, -105; -430, 0] + DstBlock "zero dynamics model" + DstPort 1 + } + } + Line { + SrcBlock "Reset Logic" + SrcPort 2 + Points [25, 0; 0, 65] + Branch { + Points [0, 170] + DstBlock "Logical\nOperator2" + DstPort 1 + } + Branch { + Points [-265, 0] + DstBlock "RIgid Impact_Model" + DstPort enable + } + } + Line { + SrcBlock "." + SrcPort 1 + DstBlock "Switch" + DstPort 1 + } + Line { + SrcBlock "Reset Logic" + SrcPort 3 + Points [10, 0; 0, 35; -165, 0; 0, 75] + DstBlock "Switch" + DstPort 2 + } + Line { + SrcBlock "Switch" + SrcPort 1 + Points [40, 0] + DstBlock "Integrator" + DstPort 3 + } + Line { + SrcBlock "Reset Logic" + SrcPort 1 + Points [35, 0; 0, 35] + DstBlock "Integrator" + DstPort 2 + } + Line { + SrcBlock "zero dynamics model" + SrcPort 1 + Points [250, 0] + DstBlock "Integrator" + DstPort 1 + } + Line { + SrcBlock "Constant1" + SrcPort 1 + DstBlock "Relational\nOperator1" + DstPort 2 + } + Line { + SrcBlock "Logical\nOperator3" + SrcPort 1 + Points [10, 0] + Branch { + DstBlock "To Workspace3" + DstPort 1 + } + Branch { + Points [0, -70] + DstBlock "Logical\nOperator1" + DstPort 2 + } + } + Line { + SrcBlock "Integrator" + SrcPort state + Points [0, -114] + DstBlock "lift zero dynamics state" + DstPort 1 + } + Line { + SrcBlock "lift zero dynamics state" + SrcPort 1 + Points [-65, 0; 0, 130] + Branch { + DstBlock "Reset Logic" + DstPort 1 + } + Branch { + Points [-140, 0] + DstBlock "RIgid Impact_Model" + DstPort 1 + } + } + Line { + SrcBlock "RIgid Impact_Model" + SrcPort 1 + Points [15, 0] + DstBlock "zero dynamics project" + DstPort 1 + } + Line { + SrcBlock "zero dynamics project" + SrcPort 1 + DstBlock "Switch" + DstPort 3 + } + Line { + SrcBlock "Logical\nOperator1" + SrcPort 1 + DstBlock "Stop Simulation" + DstPort 1 + } + Line { + SrcBlock "Logical\nOperator2" + SrcPort 1 + Points [10, 0] + Branch { + Points [0, -60] + Branch { + Points [0, -75] + DstBlock "Logical\nOperator1" + DstPort 1 + } + Branch { + DstBlock "Logical\nOperator3" + DstPort 1 + } + } + Branch { + Points [0, 15] + DstBlock "To Workspace1" + DstPort 1 + } + } + } +} diff --git a/demos/rabbit/anim/rabbit_model/zd_project.m b/demos/rabbit/anim/rabbit_model/zd_project.m new file mode 100644 index 0000000..f75903d --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_project.m @@ -0,0 +1,16 @@ +function z =zd_project(x) +%ZD_PROJECT Projects the zero dynamics to the zero dynamics. +% +% Z = ZD_PROJECT(X) + +%Eric Westervelt +%01-May-2001 07:44:48 + +q31=x(1); q32=x(2); q41=x(3); q42=x(4); q1=x(5); +dq31=x(6); dq32=x(7); dq41=x(8); dq42=x(9); dq1=x(10); + +% theta (in absolute coordinates) +z(1) = (q31+q41)/2; + +% gamma (in absolute coordinates) +z(2) = (21/4-239/250*cos(-q32+q31)+1361/250*cos(q31-q41)-41/250*cos(-q42+q31)+2/25*sin(-q1+q31)-8/5*cos(-q1+q31))*dq31+(-239/250*cos(-q32+q31)+381/500-239/250*cos(-q32+q41)+41/250*cos(q42-q32))*dq32+(1361/250*cos(q31-q41)-239/250*cos(-q32+q41)+1543/250-41/250*cos(q42-q41)-8/5*cos(q1-q41)-2/25*sin(q1-q41))*dq41+(-41/250*cos(-q42+q31)+41/250*cos(q42-q32)-41/250*cos(q42-q41)+1/10)*dq42+(2/25*sin(-q1+q31)-8/5*cos(-q1+q31)-8/5*cos(q1-q41)-2/25*sin(q1-q41)+111/50)*dq1; \ No newline at end of file diff --git a/demos/rabbit/anim/rabbit_model/zd_project_cc.m b/demos/rabbit/anim/rabbit_model/zd_project_cc.m new file mode 100644 index 0000000..21fd032 --- /dev/null +++ b/demos/rabbit/anim/rabbit_model/zd_project_cc.m @@ -0,0 +1,18 @@ +function z = zd_project_cc(x) +%ZD_PROJECT_CC Projects the zero dynamics to the zero dynamics. +% +% Z = ZD_PROJECT_CC(X) + +%Eric Westervelt +%26-Feb-2001 20:35:19 + +% first project +z = zd_project(x); +dz = zd(z); +x_zd = zd_lift(z(1),dz(1)); + +% theta +z(1) = 1/2*(x_zd(1)+x_zd(3)); + +% dtheta +z(2) = 1/2*(x_zd(6)+x_zd(8)); \ No newline at end of file diff --git a/demos/rabbit/init_clf_simulation_rabbit.m b/demos/rabbit/init_clf_simulation_rabbit.m new file mode 100644 index 0000000..485b5fc --- /dev/null +++ b/demos/rabbit/init_clf_simulation_rabbit.m @@ -0,0 +1,45 @@ +%% Initialize Global Simulation Configuration of Rabbit + +%% Control Mode: You can use various control options +params.mode.motor_dynamics = 1; % going to be supported + +%% Physical Property +params.physics.mu = 0.8; % friction constraint; + +%% Reference gait parameters. Keep this value fixed. +params.theta_end = 3.384545423940834; +params.theta_init = 2.903323878938605; +params.a_bez = [2.42225522164138,2.50944978892455,2.66152682946601,2.78780950883388,2.88091497548752,2.94002391033518;0.667715484387163,0.638300798629787,0.490574595122909,0.478548440963253,0.473893838339675,0.576633115386526;2.94201463482336,2.99945422582175,2.85290119992043,2.52843337205465,2.42762552077273,2.44116379159792;0.575072495911993,0.635317622337650,1.13813693839107,1.22909640103678,1.00446628056820,0.717766128548961]; + +%% Basic settings for the CtrlAffineSysFL +params.xdim = 14; +params.udim = 4; +params.rel_deg_y = 2; +params.use_phase = false; + +% scale changes the mass and inertial values of the robot. +% Note that the reference gait is designed for the scale of 1. +params.scale = 1; +% You can introduce an additional mass to the torso. +params.torso_add = 0; + +%% Input options +params.u_max = 150; +params.u_min = -150; + +% Be careful when tuning this! Low epsilon induces high performance, +% but it can be too aggresive to disregard some physical limitations +% and unknown dynamics +params.epsilon_FL = 0.07; % Keep it fixed! Fragile! +params.Kp_FL = 1; +params.Kd_FL = 1.8; + +%% Quadratic Programming Parameters +% objective function = A * |mu|^2 + B * |delta|^2 +% tune A and B to control the weight of each variable +params.weight.input = 1; % A (can be scalar or matrix) +params.weight.slack = 100; % B (should be scalar) + +%% Defines the initial state of the reference gait. +q0_ini = [-0.161463255651884;0.739782734886561;0.152681626367748;2.44507104015569;0.658106151373543;2.95111227794945;0.599243461678844]; +dq0_ini = [0.781190436522269;0.243063912942872;0.247793330530586;1.10287289727466;-0.548512132037030;0.203061891957879;1.50818729243256]; diff --git a/demos/rabbit/plot_rabbit_result.m b/demos/rabbit/plot_rabbit_result.m new file mode 100644 index 0000000..84362e0 --- /dev/null +++ b/demos/rabbit/plot_rabbit_result.m @@ -0,0 +1,72 @@ +function plot_rabbit_result(xs, ts, us, extras) +% Plot the result of the RABBIT model +% Argument +% xs: trajectory of RABBIT simulation +% ts: time stamp of RABBIT simulation +% us: control input of RABBIT simulation +% extras: extra logs of RABBIT simulation + +% Transpose it it to row-wise order +us = us'; +Vs = extras.Vs'; +ys = extras.ys'; +dys = extras.dys'; + +% Plot setting +subplot_gap = [0 0.1]; +fig_sz = [8 3]; +fig_sz_with_subplots = [8 4]; +plot_pos = [0 0 8 3]; + +main_color_orange = [217 83 25]/255; % r,g,b +main_color_dark_red = [162 20 47]/255; % r,g,b +main_color_blue = [0 114 189]/255; % r,g,b +main_color_green = [119 172 48]/255; + +figure(1); +plot(ts, ys(:,1)); grid on; +xlabel('Time(s)'); ylabel('q_1'); +title('Stance Leg Hip Angle'); + +figure(2); +plot(ts, ys(:,2)); grid on; +xlabel('Time(s)'); ylabel('q_3'); +title('Stance Leg Knee Angle'); + +figure(3); +plot(ts, ys(:,3)); grid on; +xlabel('Time(s)'); ylabel('q_2'); +title('Swing Leg Hip Angle'); + +figure(4); +plot(ts, ys(:,4)); grid on; +xlabel('Time(s)'); ylabel('q_4'); +title('Swing Leg Knee Angle'); + +figure(5); +plot(ts, us); grid on; hold on; +title('torques'); legend('u1','u2','u3','u4'); + +figure(6); +subplot1(2,1,'Gap', subplot_gap); +subplot1(1); hold on; +title('Euclidian Norm of the Tracking Error'); +plot(ts,sqrt(ys(:,1).^2+ys(:,2).^2+... + ys(:,3).^2+ys(:,4).^2), 'color', main_color_blue); +ylabel('||y||_2 (rad)'); grid on; +% callPlotSettings(fig_sz_with_subplots, plot_pos); + +subplot1(2); hold on; +title('Euclidian Norm of the Derivative of the Tracking Error'); +plot(ts,sqrt(dys(:,1).^2+dys(:,2).^2+... + dys(:,3).^2+dys(:,4).^2), 'color', main_color_blue); +ylabel('$||\dot{y}||_{2}$ (rad/s)','Interpreter','latex'); xlabel('Time (s)'); grid on; +% callPlotSettings(fig_sz_with_subplots, plot_pos); +%print('-f9','plot_tracking_error', '-dpng'); + +figure(7); +plot(ts, Vs); grid on; +xlabel('Time(s)'); ylabel('CLF'); +title('V(t) - t graph'); +end + diff --git a/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexa64 new file mode 100755 index 0000000..464b7e9 Binary files /dev/null and b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexmaci64 new file mode 100644 index 0000000..f192943 Binary files /dev/null and b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexw64 new file mode 100644 index 0000000..cb4ec42 Binary files /dev/null and b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/De_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper/De_five_link_walker.mexa64 new file mode 100755 index 0000000..b7e2fe6 Binary files /dev/null and b/demos/rabbit/rabbit_helper/De_five_link_walker.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/De_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/De_five_link_walker.mexmaci64 new file mode 100644 index 0000000..04a5411 Binary files /dev/null and b/demos/rabbit/rabbit_helper/De_five_link_walker.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/De_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/De_five_link_walker.mexw64 new file mode 100644 index 0000000..5d3cc92 Binary files /dev/null and b/demos/rabbit/rabbit_helper/De_five_link_walker.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexa64 new file mode 100755 index 0000000..0f11a10 Binary files /dev/null and b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexmaci64 new file mode 100644 index 0000000..65beb0d Binary files /dev/null and b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexw64 new file mode 100644 index 0000000..9938291 Binary files /dev/null and b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/J_LeftToe.mexa64 b/demos/rabbit/rabbit_helper/J_LeftToe.mexa64 new file mode 100755 index 0000000..31bb6a7 Binary files /dev/null and b/demos/rabbit/rabbit_helper/J_LeftToe.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/J_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/J_LeftToe.mexmaci64 new file mode 100644 index 0000000..476702f Binary files /dev/null and b/demos/rabbit/rabbit_helper/J_LeftToe.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/J_LeftToe.mexw64 b/demos/rabbit/rabbit_helper/J_LeftToe.mexw64 new file mode 100644 index 0000000..9a7c7f9 Binary files /dev/null and b/demos/rabbit/rabbit_helper/J_LeftToe.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/J_RightToe.mexa64 b/demos/rabbit/rabbit_helper/J_RightToe.mexa64 new file mode 100755 index 0000000..963f5aa Binary files /dev/null and b/demos/rabbit/rabbit_helper/J_RightToe.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/J_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/J_RightToe.mexmaci64 new file mode 100644 index 0000000..9d323c0 Binary files /dev/null and b/demos/rabbit/rabbit_helper/J_RightToe.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/J_RightToe.mexw64 b/demos/rabbit/rabbit_helper/J_RightToe.mexw64 new file mode 100644 index 0000000..342f89d Binary files /dev/null and b/demos/rabbit/rabbit_helper/J_RightToe.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/dJ_LeftToe.mexa64 b/demos/rabbit/rabbit_helper/dJ_LeftToe.mexa64 new file mode 100755 index 0000000..d527b99 Binary files /dev/null and b/demos/rabbit/rabbit_helper/dJ_LeftToe.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/dJ_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/dJ_LeftToe.mexmaci64 new file mode 100644 index 0000000..a6bc56a Binary files /dev/null and b/demos/rabbit/rabbit_helper/dJ_LeftToe.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/dJ_RightToe.mexa64 b/demos/rabbit/rabbit_helper/dJ_RightToe.mexa64 new file mode 100755 index 0000000..0de7ba7 Binary files /dev/null and b/demos/rabbit/rabbit_helper/dJ_RightToe.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/dJ_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/dJ_RightToe.mexmaci64 new file mode 100644 index 0000000..183317a Binary files /dev/null and b/demos/rabbit/rabbit_helper/dJ_RightToe.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/dJ_RightToe.mexw64 b/demos/rabbit/rabbit_helper/dJ_RightToe.mexw64 new file mode 100644 index 0000000..1432e4c Binary files /dev/null and b/demos/rabbit/rabbit_helper/dJ_RightToe.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/p_LeftToe.mexa64 b/demos/rabbit/rabbit_helper/p_LeftToe.mexa64 new file mode 100755 index 0000000..295e2ab Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_LeftToe.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/p_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/p_LeftToe.mexmaci64 new file mode 100644 index 0000000..7c28571 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_LeftToe.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/p_LeftToe.mexw64 b/demos/rabbit/rabbit_helper/p_LeftToe.mexw64 new file mode 100644 index 0000000..d28b320 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_LeftToe.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/p_RightToe.mexa64 b/demos/rabbit/rabbit_helper/p_RightToe.mexa64 new file mode 100755 index 0000000..085cabf Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_RightToe.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/p_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/p_RightToe.mexmaci64 new file mode 100644 index 0000000..6864828 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_RightToe.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/p_RightToe.mexw64 b/demos/rabbit/rabbit_helper/p_RightToe.mexw64 new file mode 100644 index 0000000..15e3c11 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_RightToe.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/p_Torso.mexa64 b/demos/rabbit/rabbit_helper/p_Torso.mexa64 new file mode 100755 index 0000000..dfe82f3 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_Torso.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/p_Torso.mexmaci64 b/demos/rabbit/rabbit_helper/p_Torso.mexmaci64 new file mode 100644 index 0000000..d5b86d4 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_Torso.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/p_Torso.mexw64 b/demos/rabbit/rabbit_helper/p_Torso.mexw64 new file mode 100644 index 0000000..eefacc8 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_Torso.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/p_q2_left.mexa64 b/demos/rabbit/rabbit_helper/p_q2_left.mexa64 new file mode 100755 index 0000000..eb369c1 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_q2_left.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/p_q2_left.mexmaci64 b/demos/rabbit/rabbit_helper/p_q2_left.mexmaci64 new file mode 100644 index 0000000..3739a8a Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_q2_left.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/p_q2_left.mexw64 b/demos/rabbit/rabbit_helper/p_q2_left.mexw64 new file mode 100644 index 0000000..9f95317 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_q2_left.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/p_q2_right.mexa64 b/demos/rabbit/rabbit_helper/p_q2_right.mexa64 new file mode 100755 index 0000000..68c6e27 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_q2_right.mexa64 differ diff --git a/demos/rabbit/rabbit_helper/p_q2_right.mexmaci64 b/demos/rabbit/rabbit_helper/p_q2_right.mexmaci64 new file mode 100644 index 0000000..df1e3c6 Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_q2_right.mexmaci64 differ diff --git a/demos/rabbit/rabbit_helper/p_q2_right.mexw64 b/demos/rabbit/rabbit_helper/p_q2_right.mexw64 new file mode 100644 index 0000000..5d5be7f Binary files /dev/null and b/demos/rabbit/rabbit_helper/p_q2_right.mexw64 differ diff --git a/demos/rabbit/rabbit_helper/readme_rabbit.txt b/demos/rabbit/rabbit_helper/readme_rabbit.txt new file mode 100644 index 0000000..c63a7cc --- /dev/null +++ b/demos/rabbit/rabbit_helper/readme_rabbit.txt @@ -0,0 +1,27 @@ + +How to run RABBIT simulation + +1. Add all paths under CBF-CLF-Helper + + You can use addpath(genpath(pwd)) to do it at once. + +2. Tune the uncertainty hyperparameter + + Tune plant_sys.params.scale to tune the mass-scale ratio + Tune plant_sys.params.torso_add to tune the additional weight of torso + + You can tune this values in Section in + run_clf_simulation_rabbit.m + +3. Tune the simulation setting + + Tune nsteps to control the number of steps the robot should tak + Tune dt to control the control period + + You can tune this values in Section + +4. Run run_clf_simulation_rabbit.m + +5. You can see how the robots can walk in animation form + +6. Further visualizations would be supported in future. diff --git a/demos/rabbit/rabbit_helper/trajectory_data.mat b/demos/rabbit/rabbit_helper/trajectory_data.mat new file mode 100644 index 0000000..e69d516 Binary files /dev/null and b/demos/rabbit/rabbit_helper/trajectory_data.mat differ diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m new file mode 100644 index 0000000..9b3bf85 --- /dev/null +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -0,0 +1,173 @@ +%% RABBIT MODEL SIMULATION +% WonsuhkJung smw04015@snu.ac.kr +% This is CBF-CLF Helper version of RABBIT Bipedal Walker. +% You can run the rabbit model using various controllers. You can check the +% available controllers in @CtrlAffineSysFL/ and @CtrlAffineSys/. +% This script is constitued of six sections. +% Section 1. Set simulation settings and model settings +% Section 2. Define the control system, plant system and reflect model +% uncertianty to the model. +% Section 3. Initialize the state to start simulation +% Section 4. Run simulation using rollout_controller_for_multiple_resets +% Section 5. Plot the result of the simulation +% Section 6. Animate the trajectory of the RABBIT model. +% +% Model Setting +% You can checkout init_clf_rabbit_simulation.m for model parameter +% settings. Some of them are fragile, so you might not touch it to +% reproduce the same results you've seen in paper. +% +% Simulation Setting +% dt: control period +% nstep: number of steps that RABBIT would take +% with slack: +% 0: use slack variable while solving QP +% 1: not use slack variable while solving QP +% verbose_level: +% 0: print no log (default) +% 1: print log of each steps +% 2: print also the log of the rollout +% 3: print also the log of the controller +% +% Uncertainty Setting +% You can reflect the uncertainty of the model by tuning this value. +% plant_sys.params.scale = mass-scale ratio +% plant_sys.params.torso_add = additional torso mass +% +% Controller & Sensor +% clf_qp_controller: Controller that defines the behavior in continuous +% time domain. You can define the controller you want +% to use here. +% reset_map_func: Action-definer that decides the behavior at the moment of +% reset map. In this script, we just flip the role of +% stance leg and swing leg. +% reset_event_func: Event-detector that decides the moment to reset map. +% In this script, it detects when RABBIT's left toe +% hits ground. + +close all; clear all; +%% Step 1. Set Simulation Setting and System-parameters +% Tune the simulation setting and system parameters here. +% Simulation Setting + % dt: control period of RABBIT model. + % nstep: number of step that RABBIT would take +% params: all the system-related parameters +% verbose_level: +% 0: print no log (default) +% 1: print log of each steps +% 2: print also the log of the rollout +% 3: print also the log of the controller +% with slack: +% 0: use slack variable while solving QP +% 1: not use slack variable while solving QP + +init_clf_simulation_rabbit; +dt = 0.025; +% dt = 0.002; +nstep = 10; +with_slack = true; +verbose_level = 1; + +%% Step 2. Prepare System (Uncertainty control) +control_sys = RabbitBuiltIn(params); +plant_sys = RabbitBuiltIn(params); + +% Reflect model uncertainty here +plant_sys.params.scale = 1.2; +%plant_sys.params.torso_add = 10; + +clf_qp_controller = @(x, varargin) control_sys... + .ctrlFeedbackLinearize(x, @control_sys.ctrlClfQpFL, varargin{:}); +reset_event_func = @plant_sys.rabbit_event; +reset_map_func = @plant_sys.reset_map; +exit_func = @control_sys.exit_event; + +%% Step 3. Initialize state +% Initialize the state vector. +x0 = [q0_ini;dq0_ini]; +x = x0; +t0 = 0; + +%% Step 4. Main Simulation +% Rollout the simulation +[xs, us, ts, extras] = rollout_controller_for_multiple_resets(... + x0, plant_sys, control_sys, clf_qp_controller, ... + reset_event_func, reset_map_func, nstep,... + 'with_slack', with_slack, 'verbose_level', verbose_level, ... + 'dt', dt, 'T_exit', 1, 'exclude_pre_reset', 1, 'exit_function', exit_func); +xs = xs'; +ts = ts'; + +%% Step *. Experiment +close all; +for scale = 1.0:0.1:2.0 + LfV_mismatch_array = []; + LgV_mismatch_array = []; + plant_sys = RabbitBuiltIn(params); + plant_sys.params.scale = scale; + for idx = 1:size(simul_mat, 1) + x_test = simul_mat(idx, :); + [y, dy, L2fy, LgLfy, ~] = control_sys.eval_y(x_test'); + [y_hat, dy_hat, L2fy_hat, LgLfy_hat, ~] = plant_sys.eval_y(x_test'); + + eta = [y; dy]; + delta_1 = L2fy_hat - LgLfy_hat * inv(LgLfy) * L2fy; + delta_2 = LgLfy_hat * inv(LgLfy) - eye(4); + P = control_sys.Gram_clf_FL; + G = control_sys.G_FL; + + LfV_mismatch = 2*eta'*P*G*delta_1; + LgV_mismatch = 2*eta'*P*G*delta_2; + + common = 2*eta'*P*G; + common_norm = norm(common); + + % LfV_mismatch = LfV_mismatch./common_norm; + % LgV_mismatch = LgV_mismatch./common_norm; + + LfV_mismatch_array = [LfV_mismatch_array; LfV_mismatch]; + LgV_mismatch_array = [LgV_mismatch_array; LgV_mismatch]; + end + figure() + subplot(511) + plot(perturb_head, LfV_mismatch_array); + title_descr = strcat("Mismatch term in time simulation", num2str(scale)); + title(title_descr); + x_descr = strcat("LfV"); + ylabel(x_descr); + grid on; + for i = 1:4 + subplot(5,1,i+1) + plot(perturb_head, LgV_mismatch_array(:,i)); + y_descr = strcat("LgV", num2str(i)); + ylabel(y_descr); + grid on; + end +end + +%% Sensitivity test +% (1) scale sensitivity => graph style didn't change much. +% (2) eta sensitivity +% idea: we should fix other dimension of eta, while accessing to L2fy +% freely. Is there a map from +num_axis = 12; +axis = 9; +vec = zeros(num_axis, 1); +vec(axis) = 1; +perturb_dx = 0.002; +num_step = 50; +x_temp = xs(5, :); +perturb_head = x_temp(axis) : perturb_dx : x_temp(axis) + perturb_dx * (num_step-1); +simul_mat = repmat(x_temp, num_step, 1); +simul_mat(:, axis) = perturb_head; + +%% Step 5. Plot the result +plot_rabbit_result(xs, ts, us, extras); + +%% Step 6. Five Link Animation +% This is only designed for enabling animation of bipedal walker +% global animation_scale +% animation_scale = plant_sys.params.scale; +% animation_dt = 0.05; +% x_quan_vec = coordinateTransformation(xs); +% anim_flat_ground(ts, x_quan_vec, animation_dt) \ No newline at end of file diff --git a/demos/rabbit/run_stepping_stone_rabbit.m b/demos/rabbit/run_stepping_stone_rabbit.m new file mode 100644 index 0000000..59c5542 --- /dev/null +++ b/demos/rabbit/run_stepping_stone_rabbit.m @@ -0,0 +1,159 @@ +%% RABBIT MODEL SIMULATION +% WonsuhkJung smw04015@snu.ac.kr +% This is CBF-CLF Helper version of RABBIT Bipedal Walker. +% You can run the rabbit model using various controllers. You can check the +% available controllers in @CtrlAffineSysFL/ and @CtrlAffineSys/. +% This script is constitued of six sections. +% Section 1. Set simulation settings and model settings +% Section 2. Define the control system, plant system and reflect model +% uncertianty to the model. +% Section 3. Initialize the state to start simulation +% Section 4. Run simulation using rollout_controller_for_multiple_resets +% Section 5. Plot the result of the simulation +% Section 6. Animate the trajectory of the RABBIT model. +% +% Model Setting +% You can checkout init_clf_rabbit_simulation.m for model parameter +% settings. Some of them are fragile, so you might not touch it to +% reproduce the same results you've seen in paper. +% +% Simulation Setting +% dt: control period +% nstep: number of steps that RABBIT would take +% with slack: +% 0: use slack variable while solving QP +% 1: not use slack variable while solving QP +% verbose_level: +% 0: print no log (default) +% 1: print log of each steps +% 2: print also the log of the rollout +% 3: print also the log of the controller +% +% Uncertainty Setting +% You can reflect the uncertainty of the model by tuning this value. +% plant_sys.params.scale = mass-scale ratio +% plant_sys.params.torso_add = additional torso mass +% +% Controller & Sensor +% clf_qp_controller: Controller that defines the behavior in continuous +% time domain. You can define the controller you want +% to use here. +% reset_map_func: Action-definer that decides the behavior at the moment of +% reset map. In this script, we just flip the role of +% stance leg and swing leg. +% reset_event_func: Event-detector that decides the moment to reset map. +% In this script, it detects when RABBIT's left toe +% hits ground. + +close all; clear all; +%% Step 1. Set Simulation Setting and System-parameters +% Tune the simulation setting and system parameters here. +% Simulation Setting + % dt: control period of RABBIT model. + % nstep: number of step that RABBIT would take +% params: all the system-related parameters +% verbose_level: +% 0: print no log (default) +% 1: print log of each steps +% 2: print also the log of the rollout +% 3: print also the log of the controller +% with slack: +% 0: use slack variable while solving QP +% 1: not use slack variable while solving QP + +params = init_clf_simulation_rabbit; +%% Control Barrier Function Parameters +params.cbf.rate = 50; +% Used in the cbf. +params.gamma_b = 100; +%% Settings for the Stepping stones +step_width = 0.05; +steps_min = [0.33, 0.37, 0.33]; +steps_max = steps_min + step_width; +dt = 0.002; +nstep = length(steps_min); +with_slack = true; +verbose_level = 1; + +%% Step 2. Prepare System (Uncertainty control) +params.steps_min = steps_min; +params.steps_max = steps_max; +control_sys = RabbitBuiltIn(params); +plant_sys = RabbitBuiltIn(params); + +% Reflect model uncertainty here +plant_sys.params.scale = 1.0; +%plant_sys.params.torso_add = 10; + +cbf_clf_qp_controller = @(x, varargin) control_sys... + .ctrlFeedbackLinearize(x, @control_sys.ctrlCbfClfQpFL, varargin{:}); +reset_event_func = @plant_sys.rabbit_event; +reset_map_func = @plant_sys.reset_map_with_step_update; +exit_func = @control_sys.exit_event; + +%% Step 3. Initialize state +% Initialize the state vector. +x0 = [q0_ini;dq0_ini]; +x = x0; +t0 = 0; + +%% Step 4. Main Simulation +% Rollout the simulation +[xs, us, ts, extras] = rollout_controller_for_multiple_resets(... + x0, plant_sys, control_sys, cbf_clf_qp_controller, ... + reset_event_func, reset_map_func, nstep,... + 'with_slack', with_slack, 'verbose_level', verbose_level, ... + 'dt', dt, 'T_exit', 1, 'exclude_pre_reset', 1, 'exit_function', exit_func); +extras.forces = plant_sys.get_force(xs, us); + +l_min_t_anim = []; +l_max_t_anim = []; +index_reset = [0; extras.index_reset]; +l_min_t = cell(nstep, 1); +lf_vec = []; +for i = 1:nstep + l_min_t_anim = [l_min_t_anim; 1000; ... % 1000: temporal fix for bug. + steps_min(i) * ones(index_reset(i+1)-index_reset(i)-1, 1)]; + l_min_t{i} = steps_min(i) * ones(index_reset(i+1)-index_reset(i), 1); + l_max_t_anim = [l_max_t_anim; 1000; ... % 1000: temporal fix for bug. + steps_max(i) * ones(index_reset(i+1)-index_reset(i)-1, 1)]; + l_max_t{i} = steps_max(i) * ones(index_reset(i+1)-index_reset(i), 1); +end +% for j=1:length(ts) +% +% [ds, u, FSt_u_p, FSt_nu_p, y_out, dy_out, CBF] = five_link_dynamics_cbf_clf(t_vec(j), s_vec(j, :)'); +% Fst = FSt_u_p*u + FSt_nu_p; +% Fst_vec = [Fst_vec;Fst']; +% u_vec = [u_vec;u']; +% y_out_vec = [y_out_vec; y_out']; +% dy_out_vec = [dy_out_vec; dy_out']; +% +% if control_type == 4 || control_type == 5 || control_type == 6 || control_type == 7 +% lf = p_LeftToe(s_vec(j,1:n)')-p_RightToe(s_vec(j,1:n)'); +% lf_vec = [lf_vec;lf(1)]; +% CBF_vec = [CBF_vec;CBF.']; +% end +% +% end + +extras.l_min_t = l_min_t; +extras.l_max_t = l_max_t; + +%% Step 5. Plot the result +result.trajectory = xs; +result.stamps = ts; +result.controls = us; +result.forces = extras.forces; +result.legend = "Stepping Stone (CBF)"; +plot_rabbit_state_history(result); +% plot_rabbit_result(xs, ts, us, extras); + +%% Step 6. Five Link Animation +% This is only designed for enabling animation of bipedal walker +global animation_scale +animation_scale = plant_sys.params.scale; +global SimConfig +SimConfig.m_load=0; +% animation_dt = 0.05; +% xs_vis = coordinateTransformation(xs'); +% anim_moving_stone_load(ts', xs_vis, animation_dt, l_min_t_anim, l_max_t_anim); \ No newline at end of file diff --git a/demos/rabbit/vis/plot_rabbit_state_history.m b/demos/rabbit/vis/plot_rabbit_state_history.m new file mode 100644 index 0000000..0495e79 --- /dev/null +++ b/demos/rabbit/vis/plot_rabbit_state_history.m @@ -0,0 +1,173 @@ +function fig = plot_rabbit_state_history(results, type) +%% fig = plot_rabbit_state_history(results, type) +% results: +% the structure that contains the result of a controller +% rollout, or the cell array that contains multiple rollout information. +% the structure (or each cell element) should contain the following info: +% .stamp: time history (1, T) +% .trajectory: state history (14, T) +% .forces: history of contact forces (2, T) +% .legend: title of the rollout (for instance, the name of the +% controller) that will be displayed in the plot legend. +% .Color: line color to use (optional) +% .LineWidth: line width to use (optional) +% type: type of the plot, options are +% 'simple': Display only the hip positions, swing foot positions, and the +% contact forces. +% 'extened': Display the configuration variables together. +% 'full': Display the whole state variables. + +if nargin < 2 + type = 'simple'; +end + + + palette = get_palette_colors(); + magenta = palette.magenta; + blue = palette.blue; + grey = palette.grey; + green = palette.green; + navy = palette.navy; + orange = palette.orange; + yellow = palette.yellow; + + if isstruct(results) + fig = plot_rabbit_state_history_single(results); + return + end + + n_result = numel(results); + colors_result = get_color_map([blue; yellow; magenta;], n_result-1); + + fig = open_figure('size', [1200, 900]); + + t = tiledlayout(3, 1); + title(t, "State Info"); + nexttile; + for i_result = 1:n_result + result = results{i_result}; + p = plot(result.trajectory(1, :), result.trajectory(2, :), 'DisplayName', result.legend); + p.Color = colors_result(i_result, :); + if isfield(result, 'Color') + p.Color = result.Color; + end + if isfield(result, 'LineWidth') + p.LineWidth = result.LineWidth; + end + hold on; + end + ylabel("$y_{cm}$", "Interpreter", "latex"); + xlabel("$x_{cm}$", "Interpreter", "latex"); + title('Hip Position'); + + % Swing Foot position + nexttile; + for i_result = 1:n_result + result = results{i_result}; + traj_size = size(result.trajectory); + traj_length = traj_size(2); + p_lts = []; + for i=1:traj_length + p_lt = p_LeftToe(result.trajectory(1:7, i)); + p_lts = [p_lts, p_lt([1, 3])]; + end + + p = plot(p_lts(1, :), p_lts(2, :), '.', 'DisplayName', result.legend); + p.Color = colors_result(i_result, :); + if isfield(result, 'Color') + p.Color = result.Color; + end + if isfield(result, 'LineWidth') + p.LineWidth = result.LineWidth; + end + ylabel("$y_{sw}$", "Interpreter", "latex"); + xlabel("$x_{sw}$", "Interpreter", "latex"); + hold on; + legend; + end + title('Swing Foot Position') + + % Contact Force Ratio + nexttile; + for i_result = 1:n_result + result = results{i_result}; + p = plot(result.stamps, abs(result.forces(1, :)./result.forces(2,:)),... + 'DisplayName', result.legend); + p.Color = colors_result(i_result, :); + if isfield(result, 'Color') + p.Color = result.Color; + end + if isfield(result, 'LineWidth') + p.LineWidth = result.LineWidth; + end + hold on; + end + grid on; + title('Contact Force Ratio ($|F_{T}$/$F_{N}|$)', 'Interpreter', 'latex'); + xlabel('$t$(sec)', 'Interpreter', 'latex'); + ylabel('$|F_{T}$/$F_{N}|$', 'Interpreter', 'latex'); + ylim([0, 1]); +end + +function fig = plot_rabbit_state_history_single(result) + palette = get_palette_colors(); + color_result = palette.blue; + fig = open_figure('size', [1200, 900]); + + t = tiledlayout(3, 1); + title(t, "State Info"); + nexttile; + p = plot(result.trajectory(1, :), result.trajectory(2, :)); + p.Color = color_result; + if isfield(result, 'Color') + p.Color = result.Color; + end + if isfield(result, 'LineWidth') + p.LineWidth = result.LineWidth; + end + hold on; + ylabel("$y_{cm}$", "Interpreter", "latex"); + xlabel("$x_{cm}$", "Interpreter", "latex"); + title('Hip Position'); + + % Swing Foot position + nexttile; + traj_size = size(result.trajectory); + traj_length = traj_size(2); + p_lts = []; + for i=1:traj_length + p_lt = p_LeftToe(result.trajectory(1:7, i)); + p_lts = [p_lts, p_lt([1, 3])]; + end + + p = plot(p_lts(1, :), p_lts(2, :), '.'); + p.Color = color_result; + if isfield(result, 'Color') + p.Color = result.Color; + end + if isfield(result, 'LineWidth') + p.LineWidth = result.LineWidth; + end + ylabel("$y_{sw}$", "Interpreter", "latex"); + xlabel("$x_{sw}$", "Interpreter", "latex"); + hold on; + title('Swing Foot Position') + + % Contact Force Ratio + nexttile; + p = plot(result.stamps, abs(result.forces(1, :)./result.forces(2,:))); + p.Color = color_result; + if isfield(result, 'Color') + p.Color = result.Color; + end + if isfield(result, 'LineWidth') + p.LineWidth = result.LineWidth; + end + hold on; + + grid on; + title('Contact Force Ratio ($|F_{T}$/$F_{N}|$)', 'Interpreter', 'latex'); + xlabel('$t$(sec)', 'Interpreter', 'latex'); + ylabel('$|F_{T}$/$F_{N}|$', 'Interpreter', 'latex'); + ylim([0, 1]); +end \ No newline at end of file diff --git a/demos/run_cbf_clf_simulation_2DDI.m b/demos/run_cbf_clf_simulation_2DDI.m index 13447ba..dc81e40 100644 --- a/demos/run_cbf_clf_simulation_2DDI.m +++ b/demos/run_cbf_clf_simulation_2DDI.m @@ -10,7 +10,7 @@ % obstacle radius. params.r_o = 2; -dt = 0.02; +dt = 0.002; sim_t = 30; params.cbf_gamma0 = 1; @@ -26,93 +26,58 @@ dynsys = DoubleIntegrator2D(params); -odeFun = @dynsys.dynamics; -controller = @dynsys.ctrlCbfClfQp; -odeSolver = @ode45; - -total_k = ceil(sim_t / dt); -x = x0; -t = 0; -% initialize traces. -xs = zeros(total_k, dynsys.xdim); -ts = zeros(total_k, 1); -us = zeros(total_k-1, dynsys.udim); -hs = zeros(total_k-1, 1); -Vs = zeros(total_k-1, 1); -xs(1, :) = x0'; -ts(1) = t; -u_prev = [0;0]; -for k = 1:total_k-1 - t - % Determine control input. - % dV_hat: analytic Vdot based on model. - [u, slack, h, V] = controller(x); -% [u, slack, h, V] = controller(s, u_prev); % optimizing the difference between the previous timestep. - us(k, :) = u'; - hs(k) = h; - Vs(k) = V; - - % Run one time step propagation. - [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x); - x = xs_temp(end, :)'; - - xs(k+1, :) = x'; - ts(k+1) = ts_temp(end); - u_prev = u; - t = t + dt; -end - +[xs, us, ts, extraout] = rollout_controller( ... + x0, dynsys, dynsys, @dynsys.ctrlCbfClfQp, sim_t); plot_results(ts, xs, us, params.p_o, params.r_o) - function plot_results(t, xs, us, p_o, r_o) figure subplot(5,1,1) -plot(t, xs(:,1)) +plot(t, xs(1, :)) xlabel('t') ylabel('x [m]') subplot(5,1,2) -plot(t, xs(:,2)) +plot(t, xs(2, :)) xlabel('t') ylabel('v_x [m/s]') subplot(5,1,3) -plot(t, xs(:,3)) +plot(t, xs(3, :)) xlabel('t') ylabel('y [m]') subplot(5,1,4) -plot(t, xs(:, 4)) +plot(t, xs(4, :)) xlabel('t') ylabel('v_y [m/s]') subplot(5,1,5) -plot(t, sqrt(xs(:, 2).^2 + xs(:, 4).^2)) +plot(t, sqrt(xs(2, :).^2 + xs(4, :).^2)) xlabel('t') ylabel('v [m/s]') figure subplot(2,1,1) -plot(t(1:end-1), us(:,1)) +plot(t, us(1, :)) xlabel('t') ylabel('a_x [m/s^2]') subplot(2,1,2) -plot(t(1:end-1), us(:,2)) +plot(t, us(2, :)) xlabel('t') ylabel('a_y [m/s^2]') -lim_min = min(min(xs(:, 1)), min(xs(:, 3))); -lim_max = max(max(xs(:, 1)), max(xs(:, 3))); +lim_min = min(min(xs(1, :)), min(xs(3, :))); +lim_max = max(max(xs(1, :)), max(xs(3, :))); lim_min = min([lim_min, p_o(1)-r_o, p_o(2)-r_o]); lim_max = max([lim_max, p_o(1)+r_o, p_o(2)+r_o]); figure -plot(xs(:, 1), xs(:, 3)); +plot(xs(1, :), xs(3, :)); draw_circle(p_o, r_o); xlim([lim_min, lim_max]); diff --git a/demos/run_cbf_clf_simulation_acc.m b/demos/run_cbf_clf_simulation_acc.m index b67b98e..67a7196 100644 --- a/demos/run_cbf_clf_simulation_acc.m +++ b/demos/run_cbf_clf_simulation_acc.m @@ -1,5 +1,5 @@ clear all; -close all; +% close all; dt = 0.02; sim_t = 15; @@ -34,48 +34,18 @@ params.udim = 1; %% Either option works. -accSys = AccSymbolic(params); -% accSys = AccBuiltIn(params); - -odeFun = @accSys.dynamics; -controller = @accSys.ctrlCbfClfQp; -odeSolver = @ode45; - -total_k = ceil(sim_t / dt); -x = x0; -t = 0; -% initialize traces. -xs = zeros(total_k, 3); -ts = zeros(total_k, 1); -us = zeros(total_k-1, 1); -slacks = zeros(total_k-1, 1); -hs = zeros(total_k-1, 1); -Vs = zeros(total_k-1, 1); -xs(1, :) = x0'; -ts(1) = t; -for k = 1:total_k-1 - t - Fr = accSys.getFr(x); - % Determine control input. - [u, slack, h, V] = controller(x, Fr); - us(k, :) = u'; - slacks(k, :) = slack; - hs(k) = h; - Vs(k) = V; - - % Run one time step propagation. - [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x); - x = xs_temp(end, :)'; - - xs(k+1, :) = x'; - ts(k+1) = ts_temp(end); - t = t + dt; -end +% acc_sys = AccSymbolic(params); +acc_sys = AccBuiltIn(params); -plot_results(ts, xs, us, slacks, hs, Vs, params) +[xs, us, ts, extraout] = rollout_controller( ... + x0, acc_sys, acc_sys, @acc_sys.ctrlCbfClfQp, sim_t, 'verbose_level', 1); +Vs = extraout.Vs; +Bs = extraout.Bs; +slacks = extraout.slacks; +plot_results(ts, xs, us, slacks, Bs, Vs, params); -function plot_results(ts, xs, us, slacks, hs, Vs, params) +function plot_results(ts, xs, us, slacks, Bs, Vs, params) fig_sz = [10 15]; plot_pos = [0 0 10 15]; yellow = [0.998, 0.875, 0.529]; @@ -87,7 +57,7 @@ function plot_results(ts, xs, us, slacks, hs, Vs, params) figure; subplot(6,1,1); - p = plot(ts, xs(:, 2)); + p = plot(ts, xs(2, :)); p.Color = blue; p.LineWidth = 1.5; hold on; @@ -99,7 +69,7 @@ function plot_results(ts, xs, us, slacks, hs, Vs, params) subplot(6,1,2); - p = plot(ts, xs(:, 3)); + p = plot(ts, xs(3, :)); p.Color = magenta; p.LineWidth = 1.5; ylabel("z (m)"); @@ -108,18 +78,18 @@ function plot_results(ts, xs, us, slacks, hs, Vs, params) grid on; subplot(6,1,3); - p = plot(ts(1:end-1), us); hold on; + p = plot(ts, us); hold on; p.Color = orange; p.LineWidth = 1.5; - plot(ts(1:end-1), params.u_max*ones(size(ts, 1)-1, 1), 'k--'); - plot(ts(1:end-1), params.u_min*ones(size(ts, 1)-1, 1), 'k--'); + plot(ts, params.u_max*ones(length(ts), 1), 'k--'); + plot(ts, params.u_min*ones(length(ts), 1), 'k--'); ylabel("u(N)"); title("Control Input - Wheel Force"); set(gca, 'FontSize', 14); grid on; subplot(6,1,4); - p = plot(ts(1:end-1), slacks); hold on; + p = plot(ts, slacks); hold on; p.Color = magenta; p.LineWidth = 1.5; ylabel("slack"); @@ -129,17 +99,17 @@ function plot_results(ts, xs, us, slacks, hs, Vs, params) subplot(6,1,5); - p = plot(ts(1:end-1), hs); + p = plot(ts, Bs); p.Color = navy; p.LineWidth = 1.5; - ylabel("CBF (h(x))"); + ylabel("CBF (B(x))"); title("CBF"); set(gca, 'FontSize', 14); grid on; subplot(6,1,6); set(gca, 'FontSize', 14); - p = plot(ts(1:end-1), Vs); + p = plot(ts, Vs); p.Color = navy; p.LineWidth = 1.5; xlabel("t(s)"); diff --git a/demos/run_cbf_clf_simulation_dubins_car.m b/demos/run_cbf_clf_simulation_dubins_car.m index b4a99c5..1da22aa 100644 --- a/demos/run_cbf_clf_simulation_dubins_car.m +++ b/demos/run_cbf_clf_simulation_dubins_car.m @@ -1,7 +1,7 @@ %% Example for multiple CBF constraints. clear all; close all; -dt = 0.02; +dt = 0.0005a; sim_t = 20; x0 = [0;5;0]; @@ -15,92 +15,60 @@ % Obstacle radius params.d = [2, 2]; -% params.xo = 5; -% params.yo = 4; -% params.d = 2; - params.cbf_gamma0 = 1; % Desired target point params.xd = 12; params.yd = 0; params.clf.rate = 0.5; -% params.weight.slack = 1; -% params.weight.slack = [1, 100, 100]; -params.weight.slack = [100, 100]; +params.weight_slack = 1; params.cbf.rate = 50; dubins = DubinsCar(params); -odeFun = @dubins.dynamics; -% controller = @dubins.ctrlCbfClfQp; -controller = @dubins.ctrlCbfQp; -odeSolver = @ode45; - -total_k = ceil(sim_t / dt); -x = x0; -t = 0; -% initialize traces. -xs = zeros(total_k, dubins.xdim); -ts = zeros(total_k, 1); -us = zeros(total_k-1, 1); -Vs = zeros(total_k-1, 1); -hs = zeros(total_k-1, length(params.xo)); -xs(1, :) = x0'; -ts(1) = t; -for k = 1:total_k-1 -% t - % Determine control input. - % dV_hat: analytic Vdot based on model. - [u, slack, hs_k, V] = controller(x); - us(k, :) = u'; - hs(k, :) = hs_k; - Vs(k) = V; - - % Run one time step propagation. - [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x); - x = xs_temp(end, :)'; - - xs(k+1, :) = x'; - ts(k+1) = ts_temp(end); - t = t + dt; -end +weight_slack_for_cbfs = 100 * ones(size(params.d)); + +controller = @(x, varargin) dubins.ctrlCbfClfQp(x, ... + 'weight_slack', [params.weight_slack, weight_slack_for_cbfs], varargin{:}); -plot_results(ts, xs, us, hs, [params.xo;params.yo], params.d) +[xs, us, ts, extraout] = rollout_controller( ... + x0, dubins, dubins, controller, sim_t, 'dt', dt); +Bs = extraout.Bs; +plot_results(ts, xs, us, Bs, [params.xo;params.yo], params.d) -function plot_results(t, xs, us, hs, p_o, r_o) +function plot_results(t, xs, us, Bs, p_o, r_o) figure subplot(3,1,1) -plot(t, xs(:,1)) +plot(t, xs(1, :)) xlabel('t') ylabel('x [m]') subplot(3,1,2) -plot(t, xs(:,2)) +plot(t, xs(2, :)) xlabel('t') ylabel('y [m]') subplot(3,1,3) -plot(t, xs(:,3)) +plot(t, xs(3, :)) xlabel('t') ylabel('theta [rad]') figure -plot(t(1:end-1), us) +plot(t, us) xlabel('t') ylabel('u [rad/s]') -lim_min = min(min(xs(:, 1)), min(xs(:, 2))); -lim_max = max(max(xs(:, 1)), max(xs(:, 2))); +lim_min = min(min(xs(1, :)), min(xs(2, :))); +lim_max = max(max(xs(1, :)), max(xs(2, :))); lim_min = min([lim_min, p_o(1)-r_o, p_o(2)-r_o]); lim_max = max([lim_max, p_o(1)+r_o, p_o(2)+r_o]); figure -plot(xs(:, 1), xs(:, 2)); hold on; +plot(xs(1, :), xs(2, :)); hold on; for i = 1:size(p_o, 2) draw_circle(p_o(:, i), r_o(i)); end @@ -111,11 +79,11 @@ function plot_results(t, xs, us, hs, p_o, r_o) axis equal figure -for i = 1:size(hs, 2) - subplot(size(hs, 2), 1, i) - plot(t(1:end-1), hs(:, i)) +for i = 1:size(Bs, 1) + subplot(size(Bs, 1), 1, i) + plot(t, Bs(i, :)) xlabel('t') - ylabel('cbf h(s)'); + ylabel('cbf B(s)'); end end diff --git a/demos/run_cbf_clf_simulation_quanser_cartpole.m b/demos/run_cbf_clf_simulation_quanser_cartpole.m new file mode 100644 index 0000000..510325f --- /dev/null +++ b/demos/run_cbf_clf_simulation_quanser_cartpole.m @@ -0,0 +1,76 @@ +%% Example for CBF and CLF simulation for cartpole +clear all; +close all; +dt = 0.002; +sim_t = 10; +x0 = [0;deg2rad(12);0;0]; + +params.IP02_LOAD_TYPE = 'WEIGHT'; +params.PEND_TYPE = 'LONG_24IN'; +params.u_max = 10; % max voltage +params.u_min = -10; % min voltage +params.x_lim = 0.3; +params.weight_slack = 1e3; +params.k_cbf = 10; + +params.clf.rate = 1; +params.cbf.rate = 10; + +cartPole = QuanserCartPole(params); + +controller = @(x, varargin) cartPole.ctrlCbfClfQp(x, ... + 'weight_slack', params.weight_slack, varargin{:}); + +[xs, us, ts, extraout] = rollout_controller( ... + x0, cartPole, cartPole, controller, sim_t, 'dt', dt); +Bs = extraout.Bs; +Vs = extraout.Vs; +slacks = extraout.slacks; +plot_results(ts, xs, us, Bs, Vs, slacks) + +function plot_results(t, xs, us, Bs, Vs, slacks) + +figure +subplot(4,1,1) +plot(t, xs(1, :)) +xlabel('t') +ylabel('x [m]') + +subplot(4,1,2) +plot(t, xs(2, :)) +xlabel('t') +ylabel('theta [rad]') + +subplot(4,1,3) +plot(t, xs(3, :)) +xlabel('t') +ylabel('dx [m/sec]') + +subplot(4,1,4) +plot(t, xs(4, :)) +xlabel('t') +ylabel('dtheta [rad/sec]') + +figure +subplot(3,1,1) +plot(t, Bs) +xlabel('t') +ylabel('B') + +subplot(3,1,2) +plot(t, Vs) +xlabel('t') +ylabel('V') + +subplot(3,1,3) +plot(t, slacks) +xlabel('t') +ylabel('slack') + + +figure +plot(t, us) +xlabel('t') +ylabel('u [V]') + +end \ No newline at end of file diff --git a/demos/run_clf_simulation_inverted_pendulum.m b/demos/run_clf_simulation_inverted_pendulum.m index 0af360a..e3bef8c 100644 --- a/demos/run_clf_simulation_inverted_pendulum.m +++ b/demos/run_clf_simulation_inverted_pendulum.m @@ -25,49 +25,21 @@ ip_sys = InvertedPendulum(params); -odeFun = @ip_sys.dynamics; -controller = @ip_sys.ctrlClfQp; -odeSolver = @ode45; - -total_k = ceil(sim_t / dt); -x = x0; -t = 0; -% initialize traces. -xs = zeros(total_k, ip_sys.xdim); -ts = zeros(total_k, 1); -us = zeros(total_k-1, 1); -Vs = zeros(total_k-1, 1); -xs(1, :) = x0'; -ts(1) = t; -for k = 1:total_k-1 - t - % Determine control input. - % dV_hat: analytic Vdot based on model. - [u, slack, V] = controller(x); - us(k, :) = u'; - Vs(k) = V; - - % Run one time step propagation. - [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x); - x = xs_temp(end, :)'; - - xs(k+1, :) = x'; - ts(k+1) = ts_temp(end); - t = t + dt; -end +[xs, us, ts, extraout] = rollout_controller( ... + x0, ip_sys, ip_sys, @ip_sys.ctrlClfQp, sim_t, 'verbose_level', 1); figure; title('Inverted Pendulum: CLF-QP States'); subplot(2, 1, 1); -plot(ts, 180 * xs(:, 1)/pi); +plot(ts, 180 * xs(1, :)/pi); xlabel("t (sec)"); ylabel("theta (deg)"); subplot(2, 1, 2); -plot(ts, 180 * xs(:, 2)/pi); +plot(ts, 180 * xs(2, :)/pi); xlabel("t (sec)"); ylabel("dtheta (deg/s)"); figure; -plot(ts(1:end-1), us); hold on; -plot(ts(1:end-1), params.u_max*ones(size(ts, 1)-1, 1), 'k--'); -plot(ts(1:end-1), params.u_min*ones(size(ts, 1)-1, 1), 'k--'); +plot(ts, us); hold on; +plot(ts, params.u_max*ones(size(ts, 1), 1), 'k--'); +plot(ts, params.u_min*ones(size(ts, 1), 1), 'k--'); xlabel("t (sec)"); ylabel("u (N.m)"); \ No newline at end of file diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000..d4bb2cb --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/api/classes.rst b/docs/api/classes.rst new file mode 100644 index 0000000..1e74dd3 --- /dev/null +++ b/docs/api/classes.rst @@ -0,0 +1,10 @@ +Classes +======= + +Setting up a problem with the CBF-CLF-Helper first involves making an instance of an object with the parameters of your problem. We offer the following classes for this: + +.. toctree:: + :glob: + :caption: Classes: + + classes/* \ No newline at end of file diff --git a/docs/api/classes/ctrlaffinesys.rst b/docs/api/classes/ctrlaffinesys.rst new file mode 100644 index 0000000..5f930cd --- /dev/null +++ b/docs/api/classes/ctrlaffinesys.rst @@ -0,0 +1,6 @@ +Control Affine System +===================== + +.. automodule:: lib.@CtrlAffineSys +.. autoclass:: CtrlAffineSys + :members: \ No newline at end of file diff --git a/docs/api/classes/ctrlaffinesysfl.rst b/docs/api/classes/ctrlaffinesysfl.rst new file mode 100644 index 0000000..eb8932a --- /dev/null +++ b/docs/api/classes/ctrlaffinesysfl.rst @@ -0,0 +1,6 @@ +Control Affine System Feedback Linearization +============================================ + +.. automodule:: lib.@CtrlAffineSysFL +.. autoclass:: CtrlAffineSysFL + :members: \ No newline at end of file diff --git a/docs/api/functions/sim.rst b/docs/api/functions/sim.rst new file mode 100644 index 0000000..57ea94b --- /dev/null +++ b/docs/api/functions/sim.rst @@ -0,0 +1,7 @@ +Simulation Functions +==================== + +.. automodule:: lib.sim +.. autofunction:: get_ctrl_ref_from_ctrl_previous +.. autofunction:: rollout_controller_for_multiple_resets +.. autofunction:: rollout_controller \ No newline at end of file diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 0000000..3ea5480 --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,83 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +from pathlib import Path +sys.path.append('.') +from github_linkcode import github_linkcode_resolve +# import sys +# sys.path.insert(0, os.path.abspath('.')) +# sys.path.insert(0, os.path.abspath('../ + + +# -- Project information ----------------------------------------------------- + +project = 'CBF-CLF-Helper' +copyright = '2022, Jason Choi' +author = 'Jason Choi' + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.napoleon', + 'sphinx.ext.linkcode', + 'sphinxcontrib.matlab' +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = 'sphinx_rtd_theme' + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# MATLAB +matlab_keep_package_prefix = False +matlab_src_dir = os.path.dirname(os.path.abspath('.')) +primary_domain = 'mat' +autodoc_member_order = 'bysource' +# autoclass_content = 'init' + +# Napoleon +napoleon_use_rtype = False + +# Linking to source code +def linkcode_resolve(domain, info): + return github_linkcode_resolve( + domain=domain, + info=info, + allowed_module_names=[matlab_src_dir], + github_org_id='ChoiJangho', + github_repo_id='CBF-CLF-Helper', + branch='RABBIT', + source_prefix='' + ) diff --git a/docs/github_linkcode.py b/docs/github_linkcode.py new file mode 100644 index 0000000..abf5372 --- /dev/null +++ b/docs/github_linkcode.py @@ -0,0 +1,244 @@ +"""Sphinx extension to link to source code on GitHub. +Taken from: https://github.com/opencobra/cobratoolbox/blob/master/docs/source/sphinxext/github_linkcode.py + +This links to source code for modules, classes, etc. to the correct line on +GitHub. This prevents having to bundle the source code along with the +documentation, and better ties everything together. + + +Setup +===== + +To use this, you'll need to import the module and define your own +:py:func:`linkcode_resolve` in your :file:`conf.py`:: + + from beanbag_docutils.sphinx.ext.github import github_linkcode_resolve + + extensions = [ + ... + 'sphinx.ext.linkcode', + ... + ] + + def linkcode_resolve(domain, info): + return github_linkcode_resolve( + domain=domain, + info=info, + allowed_module_names=['mymodule'], + github_org_id='myorg', + github_repo_id='myrepo', + branch='master', + source_prefix='src/') + +``source_prefix`` and ``allowed_module_names`` are optional. See the +docs for :py:func:`github_linkcode_resolve` for more information. +""" + +from __future__ import unicode_literals + +import inspect +import re +import subprocess +import sys + + +GIT_BRANCH_CONTAINS_RE = re.compile(r'^\s*([^\s]+)\s+([0-9a-f]+)\s.*') + + +_head_ref = None + + +def _run_git(cmd): + """Run git with the given arguments, returning the output. + + Args: + cmd (list of unicode): + A list of arguments to pass to :command:`git`. + + Returns: + str: + The resulting output from the command. + + Raises: + subprocess.CalledProcessError: + Error calling into git. + """ + p = subprocess.Popen(['git'] + cmd, stdout=subprocess.PIPE) + output, error = p.communicate() + ret_code = p.poll() + + if ret_code: + raise subprocess.CalledProcessError(ret_code, 'git') + + return output + + +def _git_get_nearest_tracking_branch(merge_base, remote='origin'): + """Return the nearest tracking branch for the given Git repository. + + Args: + merge_base (unicode): + The merge base used to locate the nearest tracking branch. + + remote (origin, optional): + The remote name. + + Returns: + unicode: + The nearest tracking branch, or ``None`` if not found. + """ + try: + _run_git(['fetch', 'origin', '%s:%s' % (merge_base, merge_base)]) + except Exception: + # Ignore, as we may already have this. Hopefully it won't fail later. + pass + + lines = _run_git(['branch', '-rv', '--contains', merge_base]).splitlines() + + remote_prefix = '%s/' % remote + best_distance = None + best_ref_name = None + + for line in lines: + m = GIT_BRANCH_CONTAINS_RE.match(line.strip()) + + if m: + ref_name = m.group(1) + sha = m.group(2) + + if (ref_name.startswith(remote_prefix) and + not ref_name.endswith('/HEAD')): + + distance = len(_run_git(['log', + '--pretty=format:%%H', + '...%s' % sha]).splitlines()) + + if best_distance is None or distance < best_distance: + best_distance = distance + best_ref_name = ref_name + + if best_ref_name and best_ref_name.startswith(remote_prefix): + # Strip away the remote. + best_ref_name = best_ref_name[len(remote_prefix):] + + return best_ref_name + + +def _get_git_doc_ref(branch): + """Return the commit SHA used for linking to source code on GitHub. + + The commit SHA will be cached for future lookups. + + Args: + branch (unicode): + The branch to use as a merge base. + + Returns: + unicode: + The commit SHA used for any links, if found, or ``None`` if not. + """ + global _head_ref + + if not _head_ref: + try: + tracking_branch = _git_get_nearest_tracking_branch(branch) + _head_ref = _run_git(['rev-parse', tracking_branch]).strip() + except subprocess.CalledProcessError: + _head_ref = None + + return _head_ref + + +def github_linkcode_resolve(domain, info, github_org_id, github_repo_id, + branch, source_prefix='', allowed_module_names=[]): + """Return a link to the source on GitHub for the given autodoc info. + + This takes some basic information on the GitHub project, branch, and + what modules are considered acceptable, and generates a link to the + approprite line on the GitHub repository browser for the class, function, + variable, or other object. + + Args: + domain (unicode): + The autodoc domain being processed. This only accepts "py", and + comes from the original :py:func:`linkcode_resolve` call. + + info (dict): + Information on the item being linked to. This comes from the + original :py:func:`linkcode_resolve` call. + + github_org_id (unicode): + The GitHub organization ID. + + github_repo_id (unicode): + The GitHub repository ID. + + branch (unicode): + The branch used as a merge base to find the appropriate commit + to link to. Callers may want to compute this off of the version + number of the project, or some other information. + + source_prefix (unicode, optional): + A prefix for any linked filename, in case the module is not at + the top of the source tree. + + allowed_module_names (list of unicode, optional): + The list of top-level module names considered valid for links. + If provided, links will only be generated if residing somewhere + in one of the provided module names. + """ + module_name = info['module'] + + # print domain != 'mat' + # print not module_name + # print module_name.split('.')[0] + # print allowed_module_names + # print (allowed_module_names and + # module_name.split('.')[0] not in allowed_module_names) + if (domain != 'mat' or + not module_name or + (allowed_module_names and + module_name.split('.')[0] not in allowed_module_names)): + # These aren't the modules you're looking for. + return None + + # Grab the name of the source file. + filename = module_name.replace('.', '/') + '/' + info['fullname'] + '.m' + + # Grab the module referenced in the docs. + submod = sys.modules.get(module_name) + + if submod is None: + return None + + # Split that, trying to find the module at the very tail of the module + # path. + obj = submod + + for part in info['fullname'].split('.'): + try: + obj = getattr(obj, part) + except: + return None + + # Find the line number of the thing being documented. + try: + linenum = inspect.findsource(obj)[1] + except: + linenum = None + + # Build a reference for the line number in GitHub. + if linenum: + linespec = '#L%d' % (linenum + 1) + else: + linespec = '' + + # Get the branch/tag/commit to link to. + ref = _get_git_doc_ref(branch) or branch + # print "branch: ", branch + # print ('https://github.com/%s/%s/blob/%s/%s%s%s' + # % (github_org_id, github_repo_id, ref, source_prefix, + # filename, linespec)) + return ('https://github.com/%s/%s/blob/%s/%s%s%s' + % (github_org_id, github_repo_id, ref, source_prefix, + filename, linespec)) \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000..f4a6449 --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,45 @@ +.. CBF-CLF-Helper documentation master file, created by + sphinx-quickstart on Mon Feb 14 11:07:23 2022. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + + +Welcome to CBF-CLF-Helper's documentation! +========================================== +Matlab Interface for Control Barrier Function (CBF) and Control Lyapunov Function (CLF) based control methods. The library is designed to let users easily implement safety controller based on CBFs and CLFs with Matlab. We provide: + +* An easy interface for construction and simulation of nonlinear control-affine systems. +* Safety controllers including CLF-QP, CBF-QP, and CBF-CLF-QP as built-in functions. +* Demonstrations on toy examples. + + +Requirements +------------ +- MATLAB +- `Symbolic Math Toolbox`_ + + +Usage +----- + +1. Create a class that inherit ``CtrlAffineSys``. +2. Create a class function ``defineSystem`` and define your dynamics using the symbolic toolbox. +3. Create class functions ``defineClf`` and ``defineCbf`` and define your CLF and CBF in each function respectively using the same symbolic expressions. +4. To run the simulation or run the controller, create a class instance with parameters specified as a Matlab structure array, and use the built-in functions—dynamics and other controllers such as ``ctrlCbfClfQp``, ``ctrlClfQp``, etc. + +Please checkout the `Manual`_ for more details. + +Demos +----- +Run files in the directory ``demos`` in MATLAB. + +.. Links +.. _Symbolic Math Toolbox: https://www.mathworks.com/products/symbolic.html +.. _Manual: https://github.com/HybridRobotics/CBF-CLF-Helper/blob/master/Manual_v1.pdf + +.. toctree:: + :maxdepth: 3 + :caption: Contents: + + api/classes + api/functions/sim diff --git a/docs/make.bat b/docs/make.bat new file mode 100644 index 0000000..8084272 --- /dev/null +++ b/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=. +set BUILDDIR=_build + +if "%1" == "" goto help + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.https://www.sphinx-doc.org/ + exit /b 1 +) + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/docs/requirements.txt b/docs/requirements.txt new file mode 100644 index 0000000..4edf719 --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1,3 @@ +sphinx +sphinx_rtd_theme +sphinxcontrib-matlabdomain \ No newline at end of file diff --git a/dynsys/@AccBuiltIn/AccBuiltIn.m b/dynsys/@AccBuiltIn/AccBuiltIn.m index 97e21b9..7f7ae37 100644 --- a/dynsys/@AccBuiltIn/AccBuiltIn.m +++ b/dynsys/@AccBuiltIn/AccBuiltIn.m @@ -40,7 +40,8 @@ cd = obj.params.cd; v0 = obj.params.v0; g = obj.params.g; - B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); + B = z - T * v; + %B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); end function LfB = lf_cbf(obj, x) v = x(2); diff --git a/dynsys/@AccSymbolic/defineCbf.m b/dynsys/@AccSymbolic/defineCbf.m index a6edb99..2ba2615 100644 --- a/dynsys/@AccSymbolic/defineCbf.m +++ b/dynsys/@AccSymbolic/defineCbf.m @@ -6,5 +6,6 @@ T = params.T; cd = params.cd; - cbf = z - T * v - 0.5 * (v0-v)^2 / (cd * params.g); + %cbf = z - T * v - 0.5 * (v0-v)^2 / (cd * params.g); + cbf = z - T * v; end \ No newline at end of file diff --git a/dynsys/@InvertedPendulum/defineClf.m b/dynsys/@InvertedPendulum/defineClf.m index cc3a122..95cb30f 100644 --- a/dynsys/@InvertedPendulum/defineClf.m +++ b/dynsys/@InvertedPendulum/defineClf.m @@ -1,12 +1,16 @@ function clf = defineClf(~, params, symbolic_state) x = symbolic_state; - I = params.m * params.l^2 / 3; - c_bar = params.m*params.g*params.l/(2*I); - b_bar = params.b/I; - A = [0, 1; - c_bar-params.Kp/I, -b_bar-params.Kd/I]; % Linearized Dynamics. - Q = params.clf.rate * eye(size(A,1)); - P = lyap(A', Q); % Cost Matrix for quadratic CLF. (V = e'*P*e) - clf = x' * P * x; + if isfield(params, 'P') + P = params.P; + else + I = params.m * params.l^2 / 3; + c_bar = params.m*params.g*params.l/(2*I); + b_bar = params.b/I; + A = [0, 1; + c_bar-params.Kp/I, -b_bar-params.Kd/I]; % Linearized Dynamics. + Q = params.clf.rate * eye(size(A,1)); + P = lyap(A', Q); % Cost Matrix for quadratic CLF. (V = e'*P*e) + end + clf = x' * P * x; end diff --git a/dynsys/@KinematicVehicle/KinematicVehicle.m b/dynsys/@KinematicVehicle/KinematicVehicle.m new file mode 100644 index 0000000..08c2eed --- /dev/null +++ b/dynsys/@KinematicVehicle/KinematicVehicle.m @@ -0,0 +1,49 @@ +classdef KinematicVehicle < CtrlAffineSys + methods + function [s, f, g] = defineSystem(obj, params) + % theta: heading, sigma: yaw rate + syms x y v theta sig + + s = [x; y; v; theta; sig]; + if params.unmodeled_factor.active + steer_offset = params.unmodeled_factor.steer_offset; + kv = params.unmodeled_factor.velocity_skid_coeff; + ka = params.unmodeled_factor.angular_skid_coeff; + mu = params.unmodeled_factor.friction; + + f = [v*cos(theta);v*sin(theta);-mu;v*(sig+steer_offset);0]; + % u1: longitudinal acceleration, u2: angular acceleration. + g = [0, 0; 0, 0; kv, 0; 0, 0; 0, ka]; + else + f = [v*cos(theta);v*sin(theta);0;v*sig;0]; + % u1: longitudinal acceleration, u2: angular acceleration. + g = [0, 0; 0, 0; 1, 0; 0, 0; 0, 1]; + end + end + + function clf = defineClf(~, params, symbolic_s) + x = symbolic_s(1); + y = symbolic_s(2); + v = symbolic_s(3) - params.v_target; + theta = symbolic_s(4); + sig = symbolic_s(5); + + clf = 0.37 * y^2 + 0.52 * y * theta + 3.11 * theta^2 + 0.98 * y * sig + 2.23 * sig * theta + ... + 4.46 * sig^2 - 0.36 * v * y - 0.29 * v * theta + 0.95 * v * sig + 3.86 * v^2; + end + function cbf = defineCbf(obj, params, x_sym) + xo = params.xo; + yo = params.yo; + Ro = params.Ro; + p_x = x_sym(1); + p_y = x_sym(2); + v = x_sym(3); + theta = x_sym(4); + lx = (p_x - xo)^2 + (p_y - yo)^2 - Ro^2; + + dlx = 2 * (p_x - xo) * v * cos(theta) + ... + 2 * (p_y -yo) * v * sin(theta); + cbf = dlx + params.gamma_l * lx; + end + end +end \ No newline at end of file diff --git a/dynsys/@QuanserCartPole/QuanserCartPole.m b/dynsys/@QuanserCartPole/QuanserCartPole.m new file mode 100644 index 0000000..f91ea85 --- /dev/null +++ b/dynsys/@QuanserCartPole/QuanserCartPole.m @@ -0,0 +1,140 @@ +classdef QuanserCartPole < CtrlAffineSys + methods + function obj = QuanserCartPole(params, varargin) + obj = obj@CtrlAffineSys(params, varargin{:}); + end + function [s, f_vec, g_vec] = defineSystem(obj, params) + % params: + % IP02_LOAD_TYPE: 'NO_LOAD', 'WEIGHT' + % PEND_TYPE: 'LONG_24IN', 'MEDIUM_12IN' + + % theta: heading, sigma: yaw rate + syms x theta x_dot theta_dot + + s = [x; theta; x_dot; theta_dot]; + + % Gravity + g = 9.81; + + [ Rm, Jm, Kt, eta_m, Km, Kg, eta_g, Mc, r_mp, Beq ] = ... + obj.config_ip02( params.IP02_LOAD_TYPE ); + + [ Mp, Lp, lp, Jp, Bp ] = obj.config_sp( params.PEND_TYPE ); + + % Lumped Mass of the Cart System (accounting for the rotor inertia) + Jeq = Mc + eta_g * Kg^2 * Jm / r_mp^2; + + % TODO: FINISH THIS DYNAMICS + % Dynamics computation + costheta = -cos(theta); + sintheta = -sin(theta); + + fc = eta_g*Kg*Kt/(Rm*r_mp)*(-Kg*Km*x_dot/r_mp); + gc = eta_g*Kg*Kt/(Rm*r_mp)* eta_m; + + D = [Jeq+Mp, Mp*lp*costheta ; + Mp*lp*costheta, Jp+Mp*lp^2]; + Cdq_g = [-Mp*lp*sintheta*theta_dot^2+Beq*x_dot; Mp*lp*g*sintheta+Bp*theta_dot]; + B = [1;0]; + + f_vec = [x_dot; theta_dot; D\ ((B*fc) - Cdq_g)]; + g_vec = [0; 0; D\(B*gc)]; + + end + + function [ Mp, Lp, lp, Jp, Bp ] = config_sp(obj, PEND_TYPE ) + % from Inch to Meter + K_IN2M = 0.0254; + % Set these variables (used in Simulink Diagrams) + if strcmp( PEND_TYPE, 'LONG_24IN') + % Pendulum Mass (with T-fitting) + Mp = 0.230; + % Pendulum Full Length (with T-fitting, from axis of rotation to tip) + Lp = ( 25 + 1 / 4 ) * K_IN2M; % = 0.6413; + % Distance from Pivot to Centre Of Gravity + lp = 13 * K_IN2M; % = 0.3302 + % Pendulum Moment of Inertia (kg.m^2) - approximation + Jp = Mp * Lp^2 / 12; % = 7.8838 e-3 + % Equivalent Viscous Damping Coefficient (N.m.s/rad) + Bp = 0.0024; + elseif strcmp( PEND_TYPE, 'MEDIUM_12IN') + % Pendulum Mass (with T-fitting) + Mp = 0.127; + % Pendulum Full Length (with T-fitting, from axis of rotation to tip) + Lp = ( 13 + 1 / 4 ) * K_IN2M; % = 0.3365 + % Distance from Pivot to Centre Of Gravity + lp = 7 * K_IN2M; % = 0.1778 + % Pendulum Moment of Inertia (kg.m^2) - approximation + Jp = Mp * Lp^2 / 12; % = 1.1987 e-3 + % Equivalent Viscous Damping Coefficient (N.m.s/rad) + Bp = 0.0024; + else + error( 'Error: Set the type of pendulum.' ) + end + end + + function [ Rm, Jm, Kt, eta_m, Km, Kg, eta_g, M, r_mp, Beq] = ... + config_ip02(obj, IP02_LOAD_TYPE) + % from Inch to Meter + K_IN2M = 0.0254; + % from rad/s to RPM + K_RDPS2RPM = 60 / ( 2 * pi ); + % from oz-force to N + K_OZ2N = 0.2780139; + + % Motor Armature Resistance (Ohm) + Rm = 2.6; + % Motor Torque Constant (N.m/A) + Kt = 1.088 * K_OZ2N * K_IN2M; % = .00767 + % Motor ElectroMechanical Efficiency [ = Tm * w / ( Vm * Im ) ] + eta_m = 1; + % Motor Back-EMF Constant (V.s/rad) + Km = 0.804e-3 * K_RDPS2RPM; % = .00767 + % Rotor Inertia (kg.m^2) + Jm = 5.523e-5 * K_OZ2N * K_IN2M; % = 3.9e-7 + % IP02 Cart Mass, with 3 cable connectors (kg) + Mc2 = 0.57; + % Cart Weight Mass (kg) + Mw = 0.37; + % Planetary Gearbox (a.k.a. Internal) Gear Ratio + Kg = 3.71; + % Planetary Gearbox Efficiency + eta_g = 1; + % Motor Pinion Radius (m) + r_mp = 0.5 / 2 * K_IN2M; % = 6.35e-3 + if strcmp( IP02_LOAD_TYPE, 'NO_LOAD') + M = Mc2; + Beq = 4.3; + elseif strcmp ( IP02_LOAD_TYPE, 'WEIGHT') + M = Mc2 + Mw; + Beq = 5.4; + elseif strcmp ( IP02_LOAD_TYPE, '2WEIGHTS') + M = Mc2 + 2*Mw; + Beq = 6.5; + else + error( 'Error: Set the IP02 load configuration.' ) + end + end + + function clf = defineClf(obj, params, x_sym) + x = x_sym; + [~, f_, g_] = obj.defineSystem(params); + A_sym = jacobian(f_, x); + A = double(subs(A_sym, x, zeros(4, 1))); + B = double(subs(g_, x, zeros(4, 1))); + +% Q = eye(obj.xdim); + Q = diag([100,100,0.01,0.01]); + R = obj.udim; + P = icare(A, B, Q, R); + clf = x' * P * x; +% disp(P) + end + + function cbf = defineCbf(obj, params, x_sym) + x = x_sym(1); + x_dot = x_sym(3); + cbf = -2*x*x_dot + params.k_cbf*(params.x_lim^2 - x^2); + end + end +end diff --git a/dynsys/@RabbitBuiltIn/C_gen_rel.m b/dynsys/@RabbitBuiltIn/C_gen_rel.m new file mode 100644 index 0000000..40d39a4 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/C_gen_rel.m @@ -0,0 +1,71 @@ +function C = C_gen_rel(obj, q_rel,dq_rel) + scale = obj.params.scale; + torso_mass = obj.params.torso_add; + + T = [ 0 0 -1 -1 0 0 0; + 0 0 -1 0 0 -1 0; + 0 0 -1 -1 -1 0 0; + 0 0 -1 0 0 -1 -1; + 1 0 0 0 0 0 0; + 0 1 0 0 0 0 0; + 0 0 -1 0 0 0 0]; + + q = T*q_rel + [2*pi*ones(4,1);zeros(3,1)]; + dq = T*dq_rel; + q31=q(1); q32=q(2); q41=q(3); q42=q(4); y=q(5); z=q(6); q1=q(7); + dq31=dq(1); dq32=dq(2); dq41=dq(3); dq42=dq(4); dy=dq(5); dz=dq(6); dq1=dq(7); + + % gravity + g=9.81; + + % lengths + L1=0.625; + L3=0.4; + L4=0.4; + + % mass + % M1=20*scale; + % M3=6.8*scale; + % M4=3.2*scale; + M1=12*scale + torso_mass; + M3=6.8*scale; + M4=3.2*scale; + + % com + MY1=.2; + MZ1=4.; + MZ3=1.11; + MZ4=.41; + + % link inertia + % XX1=2.22*scale; + % XX3=.25*scale; + % XX4=.10*scale; + + % XX1=1.33*scale; + % XX3=.47*scale; + % XX4=.2*scale; + + XX1=2.22*scale + 2.22*torso_mass/12; + XX3=1.08*scale; + XX4=0.93*scale; + + % C matrix + C=zeros(7); + C(1,3)=MZ4*L3*sin(-q41+q31)*dq41; + C(2,4)=MZ4*L3*sin(-q42+q32)*dq42; + C(3,1)=-MZ4*L3*sin(-q41+q31)*dq31; + C(4,2)=-MZ4*L3*sin(-q42+q32)*dq32; + C(5,1)=sin(q31)*(M4*L3+MZ3)*dq31; + C(5,2)=sin(q32)*(M4*L3+MZ3)*dq32; + C(5,3)=MZ4*sin(q41)*dq41; + C(5,4)=MZ4*sin(q42)*dq42; + C(5,7)=(sin(q1)*MZ1-cos(q1)*MY1)*dq1; + C(6,1)=-cos(q31)*(M4*L3+MZ3)*dq31; + C(6,2)=-cos(q32)*(M4*L3+MZ3)*dq32; + C(6,3)=-MZ4*cos(q41)*dq41; + C(6,4)=-MZ4*cos(q42)*dq42; + C(6,7)=(-cos(q1)*MZ1-sin(q1)*MY1)*dq1; + + C = T'*C*T; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/D_gen_rel.m b/dynsys/@RabbitBuiltIn/D_gen_rel.m new file mode 100644 index 0000000..08243d2 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/D_gen_rel.m @@ -0,0 +1,86 @@ +function D = D_gen_rel(obj,q_rel) + scale = obj.params.scale; + torso_mass = obj.params.torso_add; + + T = [ 0 0 -1 -1 0 0 0; + 0 0 -1 0 0 -1 0; + 0 0 -1 -1 -1 0 0; + 0 0 -1 0 0 -1 -1; + 1 0 0 0 0 0 0; + 0 1 0 0 0 0 0; + 0 0 -1 0 0 0 0]; + + q = T*q_rel + [2*pi*ones(4,1);zeros(3,1)]; + q31=q(1); q32=q(2); q41=q(3); q42=q(4); y=q(5); z=q(6); q1=q(7); + + % gravity + g=9.81; + + % lengths + L1=0.625; + L3=0.4; + L4=0.4; + + % mass + % M1=20*scale; + % M3=6.8*scale; + % M4=3.2*scale; + M1=12*scale + torso_mass; + M3=6.8*scale; + M4=3.2*scale; + + % com + MY1=.2; + MZ1=4.; + MZ3=1.11; + MZ4=.41; + + % link inertia + % XX1=2.22*scale; + % XX3=.25*scale; + % XX4=.10*scale; + + % XX1=1.33*scale; + % XX3=.47*scale; + % XX4=.2*scale; + + XX1=2.22*scale + 2.22*torso_mass/12; + XX3=1.08*scale; + XX4=0.93*scale; + + % D matrix + D=zeros(7); + D(1,1)=XX3+M4*L3^2; + D(1,3)=MZ4*L3*cos(-q41+q31); + D(1,5)=-cos(q31)*(M4*L3+MZ3); + D(1,6)=-sin(q31)*(M4*L3+MZ3); + D(2,2)=XX3+M4*L3^2; + D(2,4)=MZ4*L3*cos(-q42+q32); + D(2,5)=-cos(q32)*(M4*L3+MZ3); + D(2,6)=-sin(q32)*(M4*L3+MZ3); + D(3,1)=MZ4*L3*cos(-q41+q31); + D(3,3)=XX4; + D(3,5)=-MZ4*cos(q41); + D(3,6)=-MZ4*sin(q41); + D(4,2)=MZ4*L3*cos(-q42+q32); + D(4,4)=XX4; + D(4,5)=-MZ4*cos(q42); + D(4,6)=-MZ4*sin(q42); + D(5,1)=-cos(q31)*(M4*L3+MZ3); + D(5,2)=-cos(q32)*(M4*L3+MZ3); + D(5,3)=-MZ4*cos(q41); + D(5,4)=-MZ4*cos(q42); + D(5,5)=2*M3+2*M4+M1; + D(5,7)=-cos(q1)*MZ1-sin(q1)*MY1; + D(6,1)=-sin(q31)*(M4*L3+MZ3); + D(6,2)=-sin(q32)*(M4*L3+MZ3); + D(6,3)=-MZ4*sin(q41); + D(6,4)=-MZ4*sin(q42); + D(6,6)=2*M3+2*M4+M1; + D(6,7)=-sin(q1)*MZ1+cos(q1)*MY1; + D(7,5)=-cos(q1)*MZ1-sin(q1)*MY1; + D(7,6)=-sin(q1)*MZ1+cos(q1)*MY1; + D(7,7)=XX1; + + D = T'*D*T; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/G_gen_rel.m b/dynsys/@RabbitBuiltIn/G_gen_rel.m new file mode 100644 index 0000000..b380321 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/G_gen_rel.m @@ -0,0 +1,60 @@ +function G = G_gen_rel(obj, q_rel) + scale = obj.params.scale; + torso_mass = obj.params.torso_add; + T = [ 0 0 -1 -1 0 0 0; + 0 0 -1 0 0 -1 0; + 0 0 -1 -1 -1 0 0; + 0 0 -1 0 0 -1 -1; + 1 0 0 0 0 0 0; + 0 1 0 0 0 0 0; + 0 0 -1 0 0 0 0]; + + q = T*q_rel + [2*pi*ones(4,1);zeros(3,1)]; + q31=q(1); q32=q(2); q41=q(3); q42=q(4); y=q(5); z=q(6); q1=q(7); + + % gravity + g=9.81; + + % lengths + L1=0.625; + L3=0.4; + L4=0.4; + + % mass + % M1=20*scale; + % M3=6.8*scale; + % M4=3.2*scale; + M1=12*scale + torso_mass; + M3=6.8*scale; + M4=3.2*scale; + + % com + MY1=.2; + MZ1=4.; + MZ3=1.11; + MZ4=.41; + + % link inertia + % XX1=2.22*scale; + % XX3=.25*scale; + % XX4=.10*scale; + + % XX1=1.33*scale; + % XX3=.47*scale; + % XX4=.2*scale; + + XX1=2.22*scale + 2.22*torso_mass/12; + XX3=1.08*scale; + XX4=0.93*scale; + + % G matrix + G=zeros(7,1); + G(1)=-g*sin(q31)*(MZ3+L3*M4); + G(2)=-g*sin(q32)*(MZ3+L3*M4); + G(3)=-g*MZ4*sin(q41); + G(4)=-g*MZ4*sin(q42); + G(6)=g*(M1+2*M3+2*M4); + G(7)=-g*(sin(q1)*MZ1-cos(q1)*MY1); + + G = T'*G; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/L2ydq_a_gen.m b/dynsys/@RabbitBuiltIn/L2ydq_a_gen.m new file mode 100644 index 0000000..14d1e4a --- /dev/null +++ b/dynsys/@RabbitBuiltIn/L2ydq_a_gen.m @@ -0,0 +1,8 @@ +function L2ydq_a = L2ydq_a_gen(obj,in1,in2) + %L2YDQ_A_GEN + % L2YDQ_A = L2YDQ_A_GEN(IN1,IN2) + + % This function was generated by the Symbolic Math Toolbox version 8.0. + % 11-Dec-2019 16:46:17 + L2ydq_a = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0],[4,14]); +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/L2ydq_d_gen.m b/dynsys/@RabbitBuiltIn/L2ydq_d_gen.m new file mode 100644 index 0000000..bb04266 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/L2ydq_d_gen.m @@ -0,0 +1,193 @@ +function L2ydq_d = L2ydq_d_gen(obj,in1,in2,in3,th0,thf) + %L2YDQ_D_GEN + % L2YDQ_D = L2YDQ_D_GEN(IN1,IN2,IN3,TH0,THF) + + % This function was generated by the Symbolic Math Toolbox version 8.0. + % 11-Dec-2019 16:46:17 + + a01 = in3(1); + a02 = in3(2); + a03 = in3(3); + a04 = in3(4); + a11 = in3(5); + a12 = in3(6); + a13 = in3(7); + a14 = in3(8); + a21 = in3(9); + a22 = in3(10); + a23 = in3(11); + a24 = in3(12); + a31 = in3(13); + a32 = in3(14); + a33 = in3(15); + a34 = in3(16); + a41 = in3(17); + a42 = in3(18); + a43 = in3(19); + a44 = in3(20); + a51 = in3(21); + a52 = in3(22); + a53 = in3(23); + a54 = in3(24); + dq1 = in2(3,:); + dq31 = in2(4,:); + dq32 = in2(5,:); + q1 = in1(3,:); + q31 = in1(4,:); + q32 = in1(5,:); + t6 = q32.*(1.0./2.0); + t2 = q1+q31+t6-th0; + t3 = t2.^2; + t4 = th0-thf; + t5 = 1.0./t4.^5; + t10 = 1.0./t4; + t11 = t2.*t10; + t7 = t11+1.0; + t8 = t7.^2; + t9 = 1.0./t4.^2; + t12 = 1.0./t4.^3; + t13 = q1.*2.0; + t14 = q31.*2.0; + t17 = th0.*2.0; + t15 = q32+t13+t14-t17; + t16 = 1.0./t4.^4; + t18 = a41.*t2.*t3.*t5.*4.0e1; + t19 = a01.*t7.*t8.*t9.*2.0e1; + t20 = a21.*t7.*t8.*t9.*2.0e1; + t21 = a21.*t8.*t12.*t15.*6.0e1; + t22 = a21.*t3.*t7.*t16.*6.0e1; + t23 = a41.*t3.*t7.*t16.*6.0e1; + t24 = a41.*t2.*t3.*t5.*2.0e1; + t25 = a01.*t7.*t8.*t9.*1.0e1; + t26 = a21.*t7.*t8.*t9.*1.0e1; + t27 = a21.*t8.*t12.*t15.*1.5e1; + t28 = a21.*t2.*t8.*t12.*3.0e1; + t29 = a21.*t3.*t7.*t16.*3.0e1; + t30 = a41.*t3.*t7.*t16.*3.0e1; + t40 = a31.*t2.*t3.*t5.*1.0e1; + t41 = a51.*t2.*t3.*t5.*1.0e1; + t42 = a11.*t7.*t8.*t9.*2.0e1; + t43 = a11.*t2.*t8.*t12.*3.0e1; + t44 = a31.*t3.*t7.*t16.*6.0e1; + t31 = t24+t25+t26+t27+t28+t29+t30-t40-t41-t42-t43-t44-a31.*t8.*t12.*t15.*1.5e1; + t32 = dq32.*t31; + t34 = a31.*t2.*t3.*t5.*2.0e1; + t35 = a51.*t2.*t3.*t5.*2.0e1; + t36 = a11.*t7.*t8.*t9.*4.0e1; + t37 = a31.*t8.*t12.*t15.*3.0e1; + t38 = a11.*t2.*t8.*t12.*6.0e1; + t39 = a31.*t3.*t7.*t16.*1.2e2; + t33 = dq1.*(t18+t19+t20+t21+t22+t23-t34-t35-t36-t37-t38-t39); + t45 = t24+t25+t26+t27+t28+t29+t30-t40-t41-t42-t43-t44-a31.*t2.*t8.*t12.*3.0e1; + t46 = t3.^2; + t47 = t8.^2; + t48 = a41.*t5.*t46.*5.0; + t49 = a01.*t10.*t47.*5.0; + t50 = a21.*t7.*t8.*t9.*t15.*1.0e1; + t51 = a21.*t3.*t8.*t12.*3.0e1; + t52 = a41.*t2.*t3.*t7.*t16.*2.0e1; + t53 = t48+t49+t50+t51+t52-a11.*t10.*t47.*5.0-a51.*t5.*t46.*5.0-a31.*t3.*t8.*t12.*3.0e1-a11.*t2.*t7.*t8.*t9.*2.0e1-a31.*t2.*t3.*t7.*t16.*2.0e1; + t54 = a42.*t2.*t3.*t5.*4.0e1; + t55 = a02.*t7.*t8.*t9.*2.0e1; + t56 = a22.*t7.*t8.*t9.*2.0e1; + t57 = a22.*t8.*t12.*t15.*6.0e1; + t58 = a22.*t3.*t7.*t16.*6.0e1; + t59 = a42.*t3.*t7.*t16.*6.0e1; + t60 = a42.*t2.*t3.*t5.*2.0e1; + t61 = a02.*t7.*t8.*t9.*1.0e1; + t62 = a22.*t7.*t8.*t9.*1.0e1; + t63 = a22.*t8.*t12.*t15.*1.5e1; + t64 = a22.*t2.*t8.*t12.*3.0e1; + t65 = a22.*t3.*t7.*t16.*3.0e1; + t66 = a42.*t3.*t7.*t16.*3.0e1; + t76 = a32.*t2.*t3.*t5.*1.0e1; + t77 = a52.*t2.*t3.*t5.*1.0e1; + t78 = a12.*t7.*t8.*t9.*2.0e1; + t79 = a12.*t2.*t8.*t12.*3.0e1; + t80 = a32.*t3.*t7.*t16.*6.0e1; + t67 = t60+t61+t62+t63+t64+t65+t66-t76-t77-t78-t79-t80-a32.*t8.*t12.*t15.*1.5e1; + t68 = dq32.*t67; + t70 = a32.*t2.*t3.*t5.*2.0e1; + t71 = a52.*t2.*t3.*t5.*2.0e1; + t72 = a12.*t7.*t8.*t9.*4.0e1; + t73 = a32.*t8.*t12.*t15.*3.0e1; + t74 = a12.*t2.*t8.*t12.*6.0e1; + t75 = a32.*t3.*t7.*t16.*1.2e2; + t69 = dq1.*(t54+t55+t56+t57+t58+t59-t70-t71-t72-t73-t74-t75); + t81 = t60+t61+t62+t63+t64+t65+t66-t76-t77-t78-t79-t80-a32.*t2.*t8.*t12.*3.0e1; + t82 = a42.*t5.*t46.*5.0; + t83 = a02.*t10.*t47.*5.0; + t84 = a22.*t7.*t8.*t9.*t15.*1.0e1; + t85 = a22.*t3.*t8.*t12.*3.0e1; + t86 = a42.*t2.*t3.*t7.*t16.*2.0e1; + t87 = t82+t83+t84+t85+t86-a12.*t10.*t47.*5.0-a52.*t5.*t46.*5.0-a32.*t3.*t8.*t12.*3.0e1-a12.*t2.*t7.*t8.*t9.*2.0e1-a32.*t2.*t3.*t7.*t16.*2.0e1; + t88 = a43.*t2.*t3.*t5.*4.0e1; + t89 = a03.*t7.*t8.*t9.*2.0e1; + t90 = a23.*t7.*t8.*t9.*2.0e1; + t91 = a23.*t8.*t12.*t15.*6.0e1; + t92 = a23.*t3.*t7.*t16.*6.0e1; + t93 = a43.*t3.*t7.*t16.*6.0e1; + t94 = a43.*t2.*t3.*t5.*2.0e1; + t95 = a03.*t7.*t8.*t9.*1.0e1; + t96 = a23.*t7.*t8.*t9.*1.0e1; + t97 = a23.*t8.*t12.*t15.*1.5e1; + t98 = a23.*t2.*t8.*t12.*3.0e1; + t99 = a23.*t3.*t7.*t16.*3.0e1; + t100 = a43.*t3.*t7.*t16.*3.0e1; + t110 = a33.*t2.*t3.*t5.*1.0e1; + t111 = a53.*t2.*t3.*t5.*1.0e1; + t112 = a13.*t7.*t8.*t9.*2.0e1; + t113 = a13.*t2.*t8.*t12.*3.0e1; + t114 = a33.*t3.*t7.*t16.*6.0e1; + t101 = t94+t95+t96+t97+t98+t99+t100-t110-t111-t112-t113-t114-a33.*t8.*t12.*t15.*1.5e1; + t102 = dq32.*t101; + t104 = a33.*t2.*t3.*t5.*2.0e1; + t105 = a53.*t2.*t3.*t5.*2.0e1; + t106 = a13.*t7.*t8.*t9.*4.0e1; + t107 = a33.*t8.*t12.*t15.*3.0e1; + t108 = a13.*t2.*t8.*t12.*6.0e1; + t109 = a33.*t3.*t7.*t16.*1.2e2; + t103 = dq1.*(t88+t89+t90+t91+t92+t93-t104-t105-t106-t107-t108-t109); + t115 = t94+t95+t96+t97+t98+t99+t100-t110-t111-t112-t113-t114-a33.*t2.*t8.*t12.*3.0e1; + t116 = a43.*t5.*t46.*5.0; + t117 = a03.*t10.*t47.*5.0; + t118 = a23.*t7.*t8.*t9.*t15.*1.0e1; + t119 = a23.*t3.*t8.*t12.*3.0e1; + t120 = a43.*t2.*t3.*t7.*t16.*2.0e1; + t121 = t116+t117+t118+t119+t120-a13.*t10.*t47.*5.0-a53.*t5.*t46.*5.0-a33.*t3.*t8.*t12.*3.0e1-a13.*t2.*t7.*t8.*t9.*2.0e1-a33.*t2.*t3.*t7.*t16.*2.0e1; + t122 = a44.*t2.*t3.*t5.*4.0e1; + t123 = a04.*t7.*t8.*t9.*2.0e1; + t124 = a24.*t7.*t8.*t9.*2.0e1; + t125 = a24.*t8.*t12.*t15.*6.0e1; + t126 = a24.*t3.*t7.*t16.*6.0e1; + t127 = a44.*t3.*t7.*t16.*6.0e1; + t128 = a44.*t2.*t3.*t5.*2.0e1; + t129 = a04.*t7.*t8.*t9.*1.0e1; + t130 = a24.*t7.*t8.*t9.*1.0e1; + t131 = a24.*t8.*t12.*t15.*1.5e1; + t132 = a24.*t2.*t8.*t12.*3.0e1; + t133 = a24.*t3.*t7.*t16.*3.0e1; + t134 = a44.*t3.*t7.*t16.*3.0e1; + t144 = a34.*t2.*t3.*t5.*1.0e1; + t145 = a54.*t2.*t3.*t5.*1.0e1; + t146 = a14.*t7.*t8.*t9.*2.0e1; + t147 = a14.*t2.*t8.*t12.*3.0e1; + t148 = a34.*t3.*t7.*t16.*6.0e1; + t135 = t128+t129+t130+t131+t132+t133+t134-t144-t145-t146-t147-t148-a34.*t8.*t12.*t15.*1.5e1; + t136 = dq32.*t135; + t138 = a34.*t2.*t3.*t5.*2.0e1; + t139 = a54.*t2.*t3.*t5.*2.0e1; + t140 = a14.*t7.*t8.*t9.*4.0e1; + t141 = a34.*t8.*t12.*t15.*3.0e1; + t142 = a14.*t2.*t8.*t12.*6.0e1; + t143 = a34.*t3.*t7.*t16.*1.2e2; + t137 = dq1.*(t122+t123+t124+t125+t126+t127-t138-t139-t140-t141-t142-t143); + t149 = t128+t129+t130+t131+t132+t133+t134-t144-t145-t146-t147-t148-a34.*t2.*t8.*t12.*3.0e1; + t150 = a44.*t5.*t46.*5.0; + t151 = a04.*t10.*t47.*5.0; + t152 = a24.*t7.*t8.*t9.*t15.*1.0e1; + t153 = a24.*t3.*t8.*t12.*3.0e1; + t154 = a44.*t2.*t3.*t7.*t16.*2.0e1; + t155 = t150+t151+t152+t153+t154-a14.*t10.*t47.*5.0-a54.*t5.*t46.*5.0-a34.*t3.*t8.*t12.*3.0e1-a14.*t2.*t7.*t8.*t9.*2.0e1-a34.*t2.*t3.*t7.*t16.*2.0e1; + L2ydq_d = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t32+t33+dq31.*(t18+t19+t20+t21+t22+t23-a11.*t2.*t8.*t12.*6.0e1-a11.*t7.*t8.*t9.*4.0e1-a31.*t2.*t3.*t5.*2.0e1-a31.*t3.*t7.*t16.*1.2e2-a51.*t2.*t3.*t5.*2.0e1-a31.*t8.*t12.*t15.*3.0e1),t68+t69+dq31.*(t54+t55+t56+t57+t58+t59-a12.*t2.*t8.*t12.*6.0e1-a12.*t7.*t8.*t9.*4.0e1-a32.*t2.*t3.*t5.*2.0e1-a32.*t3.*t7.*t16.*1.2e2-a52.*t2.*t3.*t5.*2.0e1-a32.*t8.*t12.*t15.*3.0e1),t102+t103+dq31.*(t88+t89+t90+t91+t92+t93-a13.*t2.*t8.*t12.*6.0e1-a13.*t7.*t8.*t9.*4.0e1-a33.*t2.*t3.*t5.*2.0e1-a33.*t3.*t7.*t16.*1.2e2-a53.*t2.*t3.*t5.*2.0e1-a33.*t8.*t12.*t15.*3.0e1),t136+t137+dq31.*(t122+t123+t124+t125+t126+t127-a14.*t2.*t8.*t12.*6.0e1-a14.*t7.*t8.*t9.*4.0e1-a34.*t2.*t3.*t5.*2.0e1-a34.*t3.*t7.*t16.*1.2e2-a54.*t2.*t3.*t5.*2.0e1-a34.*t8.*t12.*t15.*3.0e1),t32+t33+dq31.*(t18+t19+t20+t21+t22+t23-t34-t35-t36-t37-t38-t39),t68+t69+dq31.*(t54+t55+t56+t57+t58+t59-t70-t71-t72-t73-t74-t75),t102+t103+dq31.*(t88+t89+t90+t91+t92+t93-t104-t105-t106-t107-t108-t109),t136+t137+dq31.*(t122+t123+t124+t125+t126+t127-t138-t139-t140-t141-t142-t143),dq1.*t45+dq31.*t45+dq32.*(t28+a01.*t7.*t8.*t9.*5.0-a11.*t2.*t8.*t12.*1.5e1-a11.*t7.*t8.*t9.*1.0e1-a31.*t2.*t3.*t5.*5.0+a21.*t7.*t8.*t9.*5.0+a21.*t3.*t7.*t16.*1.5e1+a41.*t2.*t3.*t5.*1.0e1-a31.*t2.*t8.*t12.*1.5e1-a31.*t3.*t7.*t16.*3.0e1-a51.*t2.*t3.*t5.*5.0+a41.*t3.*t7.*t16.*1.5e1),dq1.*t81+dq31.*t81+dq32.*(t64+a02.*t7.*t8.*t9.*5.0-a12.*t2.*t8.*t12.*1.5e1-a12.*t7.*t8.*t9.*1.0e1-a32.*t2.*t3.*t5.*5.0+a22.*t7.*t8.*t9.*5.0+a22.*t3.*t7.*t16.*1.5e1+a42.*t2.*t3.*t5.*1.0e1-a32.*t2.*t8.*t12.*1.5e1-a32.*t3.*t7.*t16.*3.0e1-a52.*t2.*t3.*t5.*5.0+a42.*t3.*t7.*t16.*1.5e1),dq1.*t115+dq31.*t115+dq32.*(t98+a03.*t7.*t8.*t9.*5.0-a13.*t2.*t8.*t12.*1.5e1-a13.*t7.*t8.*t9.*1.0e1-a33.*t2.*t3.*t5.*5.0+a23.*t7.*t8.*t9.*5.0+a23.*t3.*t7.*t16.*1.5e1+a43.*t2.*t3.*t5.*1.0e1-a33.*t2.*t8.*t12.*1.5e1-a33.*t3.*t7.*t16.*3.0e1-a53.*t2.*t3.*t5.*5.0+a43.*t3.*t7.*t16.*1.5e1),dq1.*t149+dq31.*t149+dq32.*(t132+a04.*t7.*t8.*t9.*5.0-a14.*t2.*t8.*t12.*1.5e1-a14.*t7.*t8.*t9.*1.0e1-a34.*t2.*t3.*t5.*5.0+a24.*t7.*t8.*t9.*5.0+a24.*t3.*t7.*t16.*1.5e1+a44.*t2.*t3.*t5.*1.0e1-a34.*t2.*t8.*t12.*1.5e1-a34.*t3.*t7.*t16.*3.0e1-a54.*t2.*t3.*t5.*5.0+a44.*t3.*t7.*t16.*1.5e1),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t53,t87,t121,t155,t53,t87,t121,t155,a01.*t10.*t47.*(5.0./2.0)-a11.*t10.*t47.*(5.0./2.0)+a41.*t5.*t46.*(5.0./2.0)-a51.*t5.*t46.*(5.0./2.0)+a21.*t3.*t8.*t12.*1.5e1-a31.*t3.*t8.*t12.*1.5e1-a11.*t2.*t7.*t8.*t9.*1.0e1+a21.*t2.*t7.*t8.*t9.*1.0e1-a31.*t2.*t3.*t7.*t16.*1.0e1+a41.*t2.*t3.*t7.*t16.*1.0e1,a02.*t10.*t47.*(5.0./2.0)-a12.*t10.*t47.*(5.0./2.0)+a42.*t5.*t46.*(5.0./2.0)-a52.*t5.*t46.*(5.0./2.0)+a22.*t3.*t8.*t12.*1.5e1-a32.*t3.*t8.*t12.*1.5e1-a12.*t2.*t7.*t8.*t9.*1.0e1+a22.*t2.*t7.*t8.*t9.*1.0e1-a32.*t2.*t3.*t7.*t16.*1.0e1+a42.*t2.*t3.*t7.*t16.*1.0e1,a03.*t10.*t47.*(5.0./2.0)-a13.*t10.*t47.*(5.0./2.0)+a43.*t5.*t46.*(5.0./2.0)-a53.*t5.*t46.*(5.0./2.0)+a23.*t3.*t8.*t12.*1.5e1-a33.*t3.*t8.*t12.*1.5e1-a13.*t2.*t7.*t8.*t9.*1.0e1+a23.*t2.*t7.*t8.*t9.*1.0e1-a33.*t2.*t3.*t7.*t16.*1.0e1+a43.*t2.*t3.*t7.*t16.*1.0e1,a04.*t10.*t47.*(5.0./2.0)-a14.*t10.*t47.*(5.0./2.0)+a44.*t5.*t46.*(5.0./2.0)-a54.*t5.*t46.*(5.0./2.0)+a24.*t3.*t8.*t12.*1.5e1-a34.*t3.*t8.*t12.*1.5e1-a14.*t2.*t7.*t8.*t9.*1.0e1+a24.*t2.*t7.*t8.*t9.*1.0e1-a34.*t2.*t3.*t7.*t16.*1.0e1+a44.*t2.*t3.*t7.*t16.*1.0e1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[4,14]); +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m new file mode 100644 index 0000000..648aed6 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -0,0 +1,193 @@ +%% Main Reference +% Aaron Ames et al. Control Barrier Function based Quadratic Programs +% with Application to Adaptive Cruise Control, CDC 2014, Table 1. + +classdef RabbitBuiltIn < CtrlAffineSysFL + properties + reset_map_matrix = [1, 0, 0, 0, 0, 0, 0; + 0, 1, 0, 0, 0, 0, 0; + 0, 0, 1, 0, 0, 0, 0; + 0, 0, 0, 0, 0, 1, 0; + 0, 0, 0, 0, 0, 0, 1; + 0, 0, 0, 1, 0, 0, 0; + 0, 0, 0, 0, 1, 0, 0]; + fall_threshold = 0.699; + theta_init + theta_end + a_bez + + %% Used for stepping stone scenario + step_min = []; + step_max = []; + steps_min = []; + steps_max = []; + step_index = []; + default_step_width = 0.05; + gamma_b + end + methods + function obj = RabbitBuiltIn(params) + % Always using built-in option for setup. + obj = obj@CtrlAffineSysFL(params, 'built-in'); + obj.theta_init = obj.params.theta_init; + obj.theta_end = obj.params.theta_end; + obj.a_bez = obj.params.a_bez; + if isfield(params, 'gamma_b') + obj.gamma_b = obj.params.gamma_b; + obj.n_cbf = 2; + else + obj.n_cbf = 0; + end + if isfield(params, 'steps_min') + obj.steps_min = params.steps_min; + if isfield(params, 'steps_max') + obj.steps_max = params.steps_max; + else + obj.steps_max = obj.steps_min + obj.default_step_width; + end + obj.step_min = obj.steps_min(1); + obj.step_max = obj.steps_max(1); + obj.step_index = 1; + elseif isfield(params, 'step_min') + obj.step_min = params.step_min; + if isfield(params, 'step_max') + obj.step_max = params.stes_max; + else + obj.step_max = obj.step_min + obj.default_step_width; + end + end + obj.n_cbf = 2; + end + + function [f, g] = defineDynamics(obj, x) + n = obj.xdim / 2; + + q = x(1:n); + dq = x(n+1:2*n); + + [D, C, G, B] = obj.get_dynamics_matrices(x); + JSt = J_RightToe(q); + JSt = JSt([1,3],:); + dJSt = dJ_RightToe(q,dq); + dJSt = dJSt([1,3],:); + + H = C*dq + G; + % Constraint Force to enforce the holonomic constraint: + FSt_u = - pinv(JSt*(D\JSt'))*(JSt*(D\B)); + FSt_nu = - pinv(JSt*(D\JSt'))*(JSt*(D\(-H)) + dJSt*dq); + + f = [dq; D\(-C*dq - G)]; + g1 = [zeros(length(dq),obj.udim); D\B]; + g2 = [zeros(length(dq),size(FSt_nu ,1)); D\JSt']; + + % dx = f(x)+g(x)u of nominal model + f = f + g2 * FSt_nu; + g = g1 + g2 * FSt_u; + end + function f_ = f(obj, x) + % RABBIT f(x) + % TODO:multiple calculation of prior procedures => if we + % maintain this architecture of different function, it is + % inevitable or else, we can have another prior function but + % still we need to waste memory space! + % memory <-> time complexity issue + [f_,~] = obj.defineDynamics(x); + end + function g_ = g(obj, x) + % RABBIT g(x) + [~, g_] = obj.defineDynamics(x); + end + function zs = z(obj, xs) + % zero dynamics + n = obj.xdim/2; + q = xs(1:n, :); + dq = xs(n+1:2*n, :); + + theta = q(3, :) + q(4, :) + q(5, :)/2; + dtheta = dq(3, :) + dq(4, :) + dq(5, :)/2; + + zs = [theta; dtheta]; + end + + function y_ = y(obj, x) + % TODO: Rabbit's output + n = obj.xdim / 2; + q = x(1:n); + theta = q(3) + q(4) + q(5)/2; + + phase = min(max((theta - obj.theta_init)/(obj.theta_end - obj.theta_init))); + + yd = obj.bezier_outputs(obj.a_bez, phase); % Not very structured actually + ya = q(4:end); + y_ = ya-yd; + end + +% function phase_ = phase(obj, x) +% % TODO: Rabbit's phase => redundant(?) +% end + + function Lfy = lf_y(obj, x) + % TODO: Not used in RABBIT + n = obj.xdim / 2; + q = x(1:n); + dq = x(n+1: 2*n); + + dydq_des = obj.dydq_d_gen(q, obj.a_bez, obj.theta_init, obj.theta_end); %[4 x 7] + dydq_act = obj.dydq_a_gen(q); + + Lfy = (dydq_act - dydq_des)*dq; % Use dy = Lfy + Lgyu = Lfy property + end + + function Lgy = lg_y(obj, x) + Lgy = zeros(obj.ydim, 1); + end + + function LgLfy = lglf_y(obj, x) + % TODO: Rabbit's lie derivative of LgLfy + n = obj.xdim / 2; + q = x(1:n); + + dydq_des = obj.dydq_d_gen(q, obj.a_bez, obj.theta_init, obj.theta_end); %[4 x 7] + dydq_act = obj.dydq_a_gen(q); + g_s = obj.g(x); + + LgLfy = (dydq_act-dydq_des)*g_s(n+1:end, :); + end + + function L2fy = l2f_y(obj, x) + % TODO: Rabbit's lie derivative of L2fy + n = obj.xdim/2; + q = x(1:n); + dq = x(n+1:2*n); + + L2ydq_des = obj.L2ydq_d_gen(q, dq, obj.a_bez, obj.theta_init, obj.theta_end); + L2ydq_act = obj.L2ydq_a_gen(q, dq); + f_s = obj.f(x); + + L2fy = (L2ydq_act - L2ydq_des)*f_s; + end + + function [D, C, G, B] = get_dynamics_matrices(obj, x) + %% [D, C, G, B] = get_dynamics_matrices(obj, x) + % D: Mass-Intertia Matrix + % C: Coriollis Matrix + % G: Gravity Vector + % B: Input Mapping Matrix + n = obj.xdim / 2; + q = x(1:n); + dq = x(n+1:2*n); + + D = obj.D_gen_rel(q); + C = obj.C_gen_rel(q,dq); % 7 x 7 + G = obj.G_gen_rel(q); + B =[0 0 0 0 + 0 0 0 0 + 0 0 0 0 + 1 0 0 0 + 0 1 0 0 + 0 0 1 0 + 0 0 0 1]; + end + end +end + diff --git a/dynsys/@RabbitBuiltIn/bezier_outputs.m b/dynsys/@RabbitBuiltIn/bezier_outputs.m new file mode 100644 index 0000000..c9ff19d --- /dev/null +++ b/dynsys/@RabbitBuiltIn/bezier_outputs.m @@ -0,0 +1,25 @@ +function fcn = bezier_outputs(obj,coeff,s) %accepts S in row or column and returns an array of the same orientation + +[n,m] = size(coeff); +[x,y] = size(s); + +m=m-1; %Bezier polynomials have m terms for m-1 order + +fcn = zeros(n,y); +for k = 0:1:m + fcn = fcn + coeff(:,k+1)*singleterm_bezier(m,k,s); +end + +return + +function val = singleterm_bezier(m,k,s) + +if (k == 0) + val = nchoosek(m,k).*(1-s).^(m-k); +elseif (m == k) + val = nchoosek(m,k)*s.^(k); +else + val = nchoosek(m,k)*s.^(k).*(1-s).^(m-k); +end + +return \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/cbf.m b/dynsys/@RabbitBuiltIn/cbf.m new file mode 100644 index 0000000..ee3cb96 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/cbf.m @@ -0,0 +1,44 @@ +function Bs = cbf(obj, x) +%% Defines two CBFs for the stepping stone problem. +% For more details, please refer to Nguyen et al., Dynamic Walking on Stepping Stones +% with Gait Library and Control Barrier Functions. + +qrel = x(1:7); +dqrel = x(8:end); + +L3=0.4; +L4=0.4; + +%% Coordinate transformation +T = [-1 -1 0 0 0; + -1 0 0 -1 0; + -1 -1 -1 0 0; + -1 0 0 -1 -1; + -1 0 0 0 0]; +q = T*qrel(3:end) + 2*pi*[ones(4,1);0]; +dq = T*dqrel(3:end); + +%% compute parameter for barrier function +l_min = obj.step_min/2; +R1 = 0.5; +R2 = 2; + +sq1=sin(q(1));sq2=sin(q(2));sq3=sin(q(3));sq4=sin(q(4));sq5=sin(q(5)); +cq1=cos(q(1));cq2=cos(q(2));cq3=cos(q(3));cq4=cos(q(4));cq5=cos(q(5)); + +lf=L3*sin(q(1))+L4*sin(q(3))-L3*sin(q(2))-L4*sin(q(4)); +dlf=L3*cos(q(1))*dq(1)+L4*cos(q(3))*dq(3)-L3*cos(q(2))*dq(2)-L4*cos(q(4))*dq(4); + +hf=-L3*cos(q(1))-L4*cos(q(3))+L3*cq2+L4*cq4; +dhf=L3*sq1*dq(1)+L4*sq3*dq(3)-L3*sq2*dq(2)-L4*sq4*dq(4); + +h1=R1+obj.step_max-sqrt(hf^2+(R1+lf)^2); +dh1=-(hf*dhf+(R1+lf)*dlf)/sqrt(hf^2+(R1+lf)^2); + +h2=sqrt((R2+hf)^2+(lf-l_min)^2)-sqrt(R2^2+l_min^2); +dh2=((R2+hf)*dhf+(lf-l_min)*dlf)/sqrt((R2+hf)^2+(lf-l_min)^2); + +gb1=obj.gamma_b*h1+dh1; +gb2=obj.gamma_b*h2+dh2; +Bs=[gb1;gb2;]; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/dq_pos_gen_v2.m b/dynsys/@RabbitBuiltIn/dq_pos_gen_v2.m new file mode 100644 index 0000000..c421456 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/dq_pos_gen_v2.m @@ -0,0 +1,20 @@ +function dq_pos = dq_pos_gen_v2(obj, x) + n = 7; + dq_pre = x(n+1:end)'; + q_pre = x(1:n)'; + scale = obj.params.scale; + + D = obj.D_gen_rel(q_pre); + Jsw = J_LeftToe(q_pre); + Jsw = Jsw([1,3],:); + + % Invertible matrix + H = [D -Jsw'; Jsw zeros(size(Jsw,1))]; + + % Linear system Hy=r + r = [D*dq_pre; zeros(size(Jsw,1),1)]; + y = H\r; + + dq_pos = y(1:length(dq_pre)); + +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/dydq_a_gen.m b/dynsys/@RabbitBuiltIn/dydq_a_gen.m new file mode 100644 index 0000000..c0c85e5 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/dydq_a_gen.m @@ -0,0 +1,9 @@ +function dydq_a = dydq_a_gen(obj,in1) + %DYDQ_A_GEN + % DYDQ_A = DYDQ_A_GEN(IN1) + + % This function was generated by the Symbolic Math Toolbox version 8.0. + % 11-Dec-2019 16:46:17 + + dydq_a = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0],[4,7]); +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/dydq_d_gen.m b/dynsys/@RabbitBuiltIn/dydq_d_gen.m new file mode 100644 index 0000000..88fedfd --- /dev/null +++ b/dynsys/@RabbitBuiltIn/dydq_d_gen.m @@ -0,0 +1,78 @@ +function dydq_d = dydq_d_gen(obj,in1,in2,th0,thf) + %DYDQ_D_GEN + % DYDQ_D = DYDQ_D_GEN(IN1,IN2,TH0,THF) + + % This function was generated by the Symbolic Math Toolbox version 8.0. + % 11-Dec-2019 16:46:04 + + a01 = in2(1); + a02 = in2(2); + a03 = in2(3); + a04 = in2(4); + a11 = in2(5); + a12 = in2(6); + a13 = in2(7); + a14 = in2(8); + a21 = in2(9); + a22 = in2(10); + a23 = in2(11); + a24 = in2(12); + a31 = in2(13); + a32 = in2(14); + a33 = in2(15); + a34 = in2(16); + a41 = in2(17); + a42 = in2(18); + a43 = in2(19); + a44 = in2(20); + a51 = in2(21); + a52 = in2(22); + a53 = in2(23); + a54 = in2(24); + q1 = in1(3,:); + q31 = in1(4,:); + q32 = in1(5,:); + t6 = q32.*(1.0./2.0); + t2 = q1+q31+t6-th0; + t3 = t2.^2; + t4 = th0-thf; + t5 = 1.0./t4.^5; + t7 = t3.^2; + t8 = 1.0./t4; + t11 = t2.*t8; + t9 = t11+1.0; + t10 = t9.^2; + t12 = t10.^2; + t13 = 1.0./t4.^3; + t14 = 1.0./t4.^2; + t15 = 1.0./t4.^4; + t16 = a41.*t5.*t7.*5.0; + t17 = a01.*t8.*t12.*5.0; + t18 = q1.*2.0; + t19 = q31.*2.0; + t25 = th0.*2.0; + t20 = q32+t18+t19-t25; + t21 = a21.*t9.*t10.*t14.*t20.*1.0e1; + t22 = a21.*t3.*t10.*t13.*3.0e1; + t23 = a41.*t2.*t3.*t9.*t15.*2.0e1; + t24 = t16+t17+t21+t22+t23-a11.*t8.*t12.*5.0-a51.*t5.*t7.*5.0-a31.*t3.*t10.*t13.*3.0e1-a11.*t2.*t9.*t10.*t14.*2.0e1-a31.*t2.*t3.*t9.*t15.*2.0e1; + t26 = a42.*t5.*t7.*5.0; + t27 = a02.*t8.*t12.*5.0; + t28 = a22.*t9.*t10.*t14.*t20.*1.0e1; + t29 = a22.*t3.*t10.*t13.*3.0e1; + t30 = a42.*t2.*t3.*t9.*t15.*2.0e1; + t31 = t26+t27+t28+t29+t30-a12.*t8.*t12.*5.0-a52.*t5.*t7.*5.0-a32.*t3.*t10.*t13.*3.0e1-a12.*t2.*t9.*t10.*t14.*2.0e1-a32.*t2.*t3.*t9.*t15.*2.0e1; + t32 = a43.*t5.*t7.*5.0; + t33 = a03.*t8.*t12.*5.0; + t34 = a23.*t9.*t10.*t14.*t20.*1.0e1; + t35 = a23.*t3.*t10.*t13.*3.0e1; + t36 = a43.*t2.*t3.*t9.*t15.*2.0e1; + t37 = t32+t33+t34+t35+t36-a13.*t8.*t12.*5.0-a53.*t5.*t7.*5.0-a33.*t3.*t10.*t13.*3.0e1-a13.*t2.*t9.*t10.*t14.*2.0e1-a33.*t2.*t3.*t9.*t15.*2.0e1; + t38 = a44.*t5.*t7.*5.0; + t39 = a04.*t8.*t12.*5.0; + t40 = a24.*t9.*t10.*t14.*t20.*1.0e1; + t41 = a24.*t3.*t10.*t13.*3.0e1; + t42 = a44.*t2.*t3.*t9.*t15.*2.0e1; + t43 = t38+t39+t40+t41+t42-a14.*t8.*t12.*5.0-a54.*t5.*t7.*5.0-a34.*t3.*t10.*t13.*3.0e1-a14.*t2.*t9.*t10.*t14.*2.0e1-a34.*t2.*t3.*t9.*t15.*2.0e1; + dydq_d = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t24,t31,t37,t43,t24,t31,t37,t43,a01.*t8.*t12.*(5.0./2.0)-a11.*t8.*t12.*(5.0./2.0)+a41.*t5.*t7.*(5.0./2.0)-a51.*t5.*t7.*(5.0./2.0)+a21.*t3.*t10.*t13.*1.5e1-a31.*t3.*t10.*t13.*1.5e1-a11.*t2.*t9.*t10.*t14.*1.0e1+a21.*t2.*t9.*t10.*t14.*1.0e1-a31.*t2.*t3.*t9.*t15.*1.0e1+a41.*t2.*t3.*t9.*t15.*1.0e1,a02.*t8.*t12.*(5.0./2.0)-a12.*t8.*t12.*(5.0./2.0)+a42.*t5.*t7.*(5.0./2.0)-a52.*t5.*t7.*(5.0./2.0)+a22.*t3.*t10.*t13.*1.5e1-a32.*t3.*t10.*t13.*1.5e1-a12.*t2.*t9.*t10.*t14.*1.0e1+a22.*t2.*t9.*t10.*t14.*1.0e1-a32.*t2.*t3.*t9.*t15.*1.0e1+a42.*t2.*t3.*t9.*t15.*1.0e1,a03.*t8.*t12.*(5.0./2.0)-a13.*t8.*t12.*(5.0./2.0)+a43.*t5.*t7.*(5.0./2.0)-a53.*t5.*t7.*(5.0./2.0)+a23.*t3.*t10.*t13.*1.5e1-a33.*t3.*t10.*t13.*1.5e1-a13.*t2.*t9.*t10.*t14.*1.0e1+a23.*t2.*t9.*t10.*t14.*1.0e1-a33.*t2.*t3.*t9.*t15.*1.0e1+a43.*t2.*t3.*t9.*t15.*1.0e1,a04.*t8.*t12.*(5.0./2.0)-a14.*t8.*t12.*(5.0./2.0)+a44.*t5.*t7.*(5.0./2.0)-a54.*t5.*t7.*(5.0./2.0)+a24.*t3.*t10.*t13.*1.5e1-a34.*t3.*t10.*t13.*1.5e1-a14.*t2.*t9.*t10.*t14.*1.0e1+a24.*t2.*t9.*t10.*t14.*1.0e1-a34.*t2.*t3.*t9.*t15.*1.0e1+a44.*t2.*t3.*t9.*t15.*1.0e1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[4,7]); +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/exit_event.m b/dynsys/@RabbitBuiltIn/exit_event.m new file mode 100644 index 0000000..dce6cb3 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/exit_event.m @@ -0,0 +1,18 @@ +function [exit_flag, last_valid] = exit_event(obj, xs) +% Exit function +% Argument +% xs : trajectory of the roobt +% Output +% exit_flag: judge if robot has fallen +% last_valid: return the last valid index of the trajectory + +height_threshold = obj.fall_threshold; +height_trajectory = xs(2, :); +last_valid = find(height_trajectory 0 + break + end +end +end +function constr= fzeroFunc(z0, x0) + yInit = z0(1); + qSwingKneeInit = z0(2); + dxInit = z0(3); + dyInit = z0(4); + + qInit = x0(1:7); + qInit(2) = yInit; + qInit(7) = qSwingKneeInit; + pFootStance = p_RightToe(qInit); + + dqInit = x0(8:14); + dqInit(1) = dxInit; + dqInit(2) = dyInit; + + vFootStance = J_RightToe(qInit)*dqInit; + vFootStance = vFootStance([1,3]); + + % forward kinematics constraint for pfoot, vfoot + % => constraining all the belows are 0 + constr = [pFootStance(3); + vFootStance]; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/rabbit_event.m b/dynsys/@RabbitBuiltIn/rabbit_event.m new file mode 100644 index 0000000..c3fa390 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/rabbit_event.m @@ -0,0 +1,16 @@ +function [value, isterminal, direction] = rabbit_event(obj,t, s) +% Define the event here to partition the simulation +% according to event +% RABBIT_EVENT: solve ode45 until the Rabbit's LeftToe +% touches ground +% Based on left toe position + +x = s(1:7); +p_lt = p_LeftToe(x); +value = p_lt(3); + +isterminal = 1; +direction = -1; + +end + diff --git a/dynsys/@RabbitBuiltIn/reset_map.m b/dynsys/@RabbitBuiltIn/reset_map.m new file mode 100644 index 0000000..92b8af1 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/reset_map.m @@ -0,0 +1,21 @@ +function x_after = reset_map(obj, x_before, varargin) +%% Reset Map Function +% varargin should have the environment setting + + % TODO: generalize this to environment setting class. + environment = parse_function_args(varargin{:}); + + if ~isfield(environment, 'perturbation') + perturbation = 0; + else + perturbation = environment.perturbation; + end + + n = obj.xdim/2; + q = obj.reset_map_matrix * x_before(1:n)'; + dq = obj.reset_map_matrix * obj.dq_pos_gen_v2([x_before(1:n) x_before(n+1:2*n)]); + x_after = [q; dq]; + if perturbation ~= 0 + x_after = obj.perturb(x_after, perturbation); % not neat though. + end +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/reset_map_with_step_update.m b/dynsys/@RabbitBuiltIn/reset_map_with_step_update.m new file mode 100644 index 0000000..edaa68e --- /dev/null +++ b/dynsys/@RabbitBuiltIn/reset_map_with_step_update.m @@ -0,0 +1,29 @@ +function x_after = reset_map_with_step_update(obj, x_before, varargin) +%% Reset Map Function +% varargin should have the environment setting + + % TODO: generalize this to environment setting class. + environment = parse_function_args(varargin{:}); + + if ~isfield(environment, 'perturbation') + perturbation = 0; + else + perturbation = environment.perturbation; + end + + n = obj.xdim/2; + q = obj.reset_map_matrix * x_before(1:n)'; + dq = obj.reset_map_matrix * obj.dq_pos_gen_v2([x_before(1:n) x_before(n+1:2*n)]); + x_after = [q; dq]; + if perturbation ~= 0 + x_after = obj.perturb(x_after, perturbation); % not neat though. + end + + if ~isempty(obj.steps_min) + if obj.step_index < obj.steps_min + obj.step_index = obj.step_index + 1; + obj.step_min = obj.steps_min(obj.step_index); + obj.step_max = obj.steps_max(obj.step_index); + end + end +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/updateAllProperty.m b/dynsys/@RabbitBuiltIn/updateAllProperty.m new file mode 100644 index 0000000..b9a5c28 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/updateAllProperty.m @@ -0,0 +1,8 @@ +function [outputArg1,outputArg2] = updateAllProperty(inputArg1,inputArg2) +% Need for further optimization in calculation +%UPDATEALLPROPERTY 이 함수의 요약 설명 위치 +% 자세한 설명 위치 +outputArg1 = inputArg1; +outputArg2 = inputArg2; +end + diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index a747063..8dbe320 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -1,56 +1,77 @@ %% Author: Jason Choi (jason.choi@berkeley.edu) classdef CtrlAffineSys < handle %% Control-Affine Dynamic System Class. + % More comments may be present in source code + properties - % Set-up option: 'symbolic', 'built-in' - setup_option + setup_option % Set-up option: 'symbolic', 'built-in' - % necessary parameters - % rate used in the clf constraint. - clf_rate - % rate used in the cbf constraint. - cbf_rate - % max bound of control input. - u_max - % min bound of control input. - u_min + %% necessary parameters + + clf_rate % rate used in the clf constraint. + cbf_rate % rate used in the cbf constraint. + + u_max % max bound of control input. + u_min % min bound of control input. - % State dimension - xdim - % Control input dimension - udim - % Number of clf in use - n_clf - % Number of cbf in use - n_cbf + xdim % State dimension + sdim % + udim % Control input dimension + n_clf % Number of clf in use + n_cbf % Number of cbf in use + + %% weights used in the built-in controllers + + weight_input % weight on control input + weight_slack % weight on slack variable % Model parameters as a structure object, specific to the system. % This might contain other parameters used for setting up the dynsys, - % for instance, clf_rate and cbf_rate. However, referring to this + % for instance, ``clf_rate`` and ``cbf_rate``. However, referring to this % variable's fields directly instead of the equivalent class variables % (when they exist) is not recommended. % (Always preferred to refer directly to the class properties.) - params + params %% Functions generated from symbolic expressions. % (Used when setup_option is 'symbolic'.) - f_sym - g_sym - cbf_sym - lf_cbf_sym - lg_cbf_sym - clf_sym - lf_clf_sym - lg_clf_sym + + f_sym % :math:`f` function generated from symbolic expression + g_sym % :math:`g` function generated from symbolic expression + cbf_sym % CBF function generated from symbolic expression + lf_cbf_sym % :math:`L_f B(x)` function generated from symbolic expression + lg_cbf_sym % :math:`L_g B(x)` function generated from symbolic expression + clf_sym % CLF function generated from symbolic expression + lf_clf_sym % :math:`L_f V(x)` function generated from symbolic expression + lg_clf_sym % :math:`L_g V(x)` function generated from symbolic expression end methods function obj = CtrlAffineSys(params, setup_option) - %% dynsys = CtrlAffineSys(params) - %% default: - %% dynsys = CtrlAffineSys(params, 'symbolic') - %% if user-defined dynamics, cbf, clf are used: - %% dynsys = CtrlAffineSys(params, 'built-in') + % | Define a dynamical system through the ``CtrlAffineSys`` constructor + % | ``dynsys = CtrlAffineSys(params)`` + % + % Examples: + % default usage: + % ``dynsys = CtrlAffineSys(params, 'symbolic')`` + % if user-defined dynamics, cbf, clf are used: + % ``dynsys = CtrlAffineSys(params, 'built-in')`` + % + % Args: + % params: dictionary of necessary parameters to define the dynsys + % you can pass the model parameters you want to use to define + % the dynamics, together with the below fields that is necessary + % to support the functionality of the library. + % + % Necessary fields: + % | ``clf_rate`` / ``clf.rate``: rate for the clf constraint. + % | ``cbf_rate`` / ``cbf.rate``: rate for the cbf constraint. + % For the 'built-in' option: + % | ``xdim``: state dimension + % | ``udim``: control input dimension + % | ``u_min``: control min bound (scalar, or the vector of size u_min) + % | ``u_max``: control max bound + % setup_option: ``'symbolic'`` or ``'builtin'`` if nargin < 1 error("Warning: params argument is missing.") end @@ -63,23 +84,33 @@ setup_option = 'built-in'; end obj.setup_option = setup_option; - obj.init_sys(params); + obj.init_sys(params); end function [x, f, g] = defineSystem(obj, params) - % [x, f, g] = defineSystem(obj, params) - % For 'symbolic' setup, use this to define your dynamics, - % Outputs: x: symbolic state vector - % f: drift term, expressed symbolically wrt x. - % g: control vector fields, expressed symbolically wrt x. + % | For 'symbolic' setup, use this to define your dynamics + % | ``[x, f, g] = defineSystem(obj, params)`` + % + % Returns: + % [x, f, g]: + % + % x: symbolic state vector. + % + % f: drift term, expressed symbolically wrt x. + % + % g: control vector fields, expressed symbolically wrt x. + % x = []; f = []; g = []; end function clf = defineClf(obj, params, symbolic_state) - % clf = defineClf(obj, params, symbolic_state) % For 'symbolic' setup, use this to define the CLF. - % symbolic state: same symbolic state created in defineSystem - % clf: CLF expressed symbolically wrt symbolic_state. + % ``clf = defineClf(obj, params, symbolic_state)`` + % + % Args: + % symbolic_state: same symbolic state created in defineSystem + % + % CLF expressed symbolically wrt symbolic_state. clf = []; end @@ -95,15 +126,18 @@ % dx = obj.dynamics(t, x, u) % Control-Affine Dynamics: xdot = f(x) + g(x) u % Inputs: t: time, x: state, u: control input - % Output: dx: \dot(x) + % x can be multiple element (size: (xdim, n_element)) + % u can be multiple element (size: (udim, n_element)) + % Output: dx: \dot(x) (size: (xdim, n_element)) dx = obj.f(x) + obj.g(x) * u; end function f_ = f(obj, x) - % f_ = f(obj, x) + % f_ = obj.f(x) % For 'built-in' setup, override this function with the % user-defined implementation of f(x). % Autonomous vector fields (or drift). + % x can be multiple element (size: (xdim, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, f(x) should be overriden by user."); end @@ -111,99 +145,204 @@ end function g_ = g(obj, x) - % g_ = g(obj, x) + % g_ = obj.g(x) % For 'built-in' setup, override this function with the % user-defined implementation of g(x). % Control vector fields (or actuation effect). + % x can be multiple element (size: (xdim, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.g(x) should be overriden by user."); end g_ = obj.g_sym(x); end - function clf_ = clf(obj, x) - % clf_ = clf(obj, x) + function Vs = clf(obj, x) + % Vs = obj.clf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the Control Lyapunov Function V(x). + % x can be multiple elements (size: (xdim, n_element)) + % Vs: (size: (obj.n_clf, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.clf(x) should be overriden by user."); end - clf_ = zeros(obj.n_clf, 1); + n_states = size(x, 2); + Vs = zeros(obj.n_clf, n_states); for i = 1:obj.n_clf clf_i = obj.clf_sym{i}(x); - clf_(i) = clf_i; + Vs(i, :) = clf_i; end end - function lf_clf_ = lf_clf(obj, x) - % lf_clf_ = lf_clf(obj, x) + function LfVs = lf_clf(obj, x) + % LfVs = obj.lf_clf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the lie derivative of the CLF L_f{V(x)}. + % x can be multiple elements (size: (xdim, n_element)) + % LfVs: (size: (obj.n_clf, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.lf_clf(x) should be overriden by user."); end - lf_clf_ = zeros(obj.n_clf, 1); + n_states = size(x, 2); + LfVs = zeros(obj.n_clf, n_states); for i = 1:obj.n_clf lf_clf_i = obj.lf_clf_sym{i}(x); - lf_clf_(i) = lf_clf_i; + LfVs(i, :) = lf_clf_i; end end - function lg_clf_ = lg_clf(obj, x) - % lg_clf_ = lg_clf(obj, x) + function LgVs = lg_clf(obj, x) + % LgVs = obj.lg_clf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the lie derivative of the CLF L_g{V(x)}. + % x can be multiple elements (size: (xdim, n_element)) + % LgVs: (size: (obj.n_clf, obj.udim, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.lg_clf(x) should be overriden by user."); end - lg_clf_ = zeros(obj.n_clf, obj.udim); + n_states = size(x, 2); + LgVs = zeros(obj.n_clf, obj.udim, n_states); for i = 1:obj.n_clf - lg_clf_i = obj.lg_clf_sym{i}(x); - lg_clf_(i, :) = lg_clf_i; + lg_clf_i = reshape(obj.lg_clf_sym{i}(x), [], obj.udim)'; + LgVs(i, :, :) = lg_clf_i; end end - function cbf_ = cbf(obj, x) - % cbf_ = cbf(obj, x) + function Vdots = dclf(obj, x, u) + % Vdots = obj.dclf(x, u) + % Model based estimate of the Lie derivatives of CLF + % x can be multiple elements (size: (xdim, n_element)) + % u can be multiple elements (size: (udim, n_element)) + % Output size: (obj.n_clf, n_element) + u = reshape(u, [1, size(u)]); + u = permute(u, [2, 1, 3]); + LfVs = obj.lf_clf(x); + LgVs = obj.lg_clf(x); + Vdots = LfVs + reshape(pagemtimes(LgVs, u), obj.n_clf, []); + end + + function Bs = cbf(obj, x) + % Bs = obj.cbf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the Control Barrier Function B(x). + % x can be multiple elements (size: (xdim, n_element)) + % Bs: (size: (obj.n_cbf, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.cbf(x) should be overriden by user."); end - cbf_ = zeros(obj.n_cbf, 1); + n_states = size(x, 2); + Bs = zeros(obj.n_cbf, n_states); for i = 1:obj.n_cbf cbf_i = obj.cbf_sym{i}(x); - cbf_(i) = cbf_i; + Bs(i, :) = cbf_i; end end - function lf_cbf_ = lf_cbf(obj, x) - % lf_cbf_ = lf_cbf(obj, x) + function LfBs = lf_cbf(obj, x) + % LfBs = obj.lf_cbf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the lie derivative of the CBF L_f{B(x)}. + % x can be multiple elements (size: (xdim, n_element)) + % LfBs: (size: (obj.n_cbf, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.lf_cbf(x) should be overriden by user."); end - lf_cbf_ = zeros(obj.n_cbf, 1); + n_states = size(x, 2); + LfBs = zeros(obj.n_cbf, n_states); for i = 1:obj.n_cbf lf_cbf_i = obj.lf_cbf_sym{i}(x); - lf_cbf_(i) = lf_cbf_i; + LfBs(i, :) = lf_cbf_i; end end - function lg_cbf_ = lg_cbf(obj, x) - % lg_cbf_ = lg_cbf(obj, x) + function LgBs = lg_cbf(obj, x) + % LgBs = obj.lg_cbf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + % x can be multiple elements (size: (xdim, n_element)) + % LgBs: (size: (obj.n_cbf, obj.udim, n_element)) if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.lg_cbf(x) should be overriden by user."); end - lg_cbf_ = zeros(obj.n_cbf, obj.udim); + n_states = size(x, 2); + LgBs = zeros(obj.n_cbf, obj.udim, n_states); for i = 1:obj.n_cbf - lg_cbf_i = obj.lg_cbf_sym{i}(x); - lg_cbf_(i, :) = lg_cbf_i; + lg_cbf_i = reshape(obj.lg_cbf_sym{i}(x), [], obj.udim)'; + LgBs(i, :, :) = lg_cbf_i; end - end + end + + function Bdots = dcbf(obj, x, u) + % Bdots = obj.dcbf(x, u) + % Model based estimate of the Lie derivatives of CBF + % x can be multiple elements (size: (xdim, n_element)) + % u can be multiple elements (size: (udim, n_element)) + % Output size: (obj.n_cbf, n_element) + u = reshape(u, [1, size(u)]); + u = permute(u, [2, 1, 3]); + LfBs = obj.lf_cbf(x); + LgBs = obj.lg_cbf(x); + Bdots = LfBs + reshape(pagemtimes(LgBs, u), obj.n_cbf, []); + end + + function train_data = reconstruct_train_data(obj, xs, us, varargin) + % This reconstruction method adapts following philosophy + % dV = V(t+1) - V(t) / dt + % dV_hat = (LfV(x_t) + LfV(x_{t+1}))/2 + (LgV(x_t) + + % LgV(x_{t+1})/2 * u + % dV_error = dV - dV_hat + % There might be an alternative to evaluate + % LfV((x_t+x_{t+1})/2) rather than the above method. + settings = parse_function_args(varargin{:}); + + % Sanity check + [x_dim, num_data] = size(xs); + [u_dim, num_data_u] = size(us); + if x_dim ~= obj.xdim || u_dim ~= obj.udim + error("Your data format is wrong. Make sure that your input abides by right format"); + end + if isfield(settings, 'ts') + ts = settings.ts; + elseif isfield(settings, 'dt') + ts = 0:settings.dt:(num_data-1)*settings.dt; + else + error("You must provide time information to reconstruct training data"); + end + + reconstruct_clf = isfield(settings, 'Vs'); + reconstruct_cbf = isfield(settings, 'Bs'); + + if reconstruct_clf + Vs = settings.Vs; + dV_hats = zeros(1, num_data - 1); + for i = 1:num_data-1 + x = xs(:, i); u = us(:, i); + x_next = xs(:, i+1); + dV_hat = obj.lf_clf(x) + obj.lg_clf(x) * u; + dV_hat_next = obj.lf_clf(x_next) + obj.lg_clf(x_next) * u; + dV_hats(i) = (dV_hat + dV_hat_next)/2; + end + dVs = (Vs(2:end) - Vs(1:end-1))./(ts(2:end) - ts(1:end-1)); + dVs_error = dVs - dV_hats; + + train_data.dVs_error = dVs_error; + end + + if reconstruct_cbf + Bs = settings.Bs; + dB_hats = zeros(1, num_data-1); + for i = 1:num_data-1 + x = xs(:, i); u = us(:, i); + x_next = xs(:, i+1); + dB_hat = obj.lf_cbf(x) + obj.lg_cbf(x) * u; + dB_hat_next = obj.lf_cbf(x_next) + obj.lg_cbf(x_next) * u; + dB_hats(i) = (dB_hat + dB_hat_next)/2; + end + dBs = (Bs(2:end) - Bs(1:end-1))./(ts(2:end) - ts(1:end-1)); + dBs_error = dBs - dB_hats; + + train_data.dBs_error = dBs_error; + end + end end end diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index b2aee29..affa391 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -1,17 +1,20 @@ -%% Author: Jason Choi (jason.choi@berkeley.edu) -function [u, slack, Bs, Vs, feas, comp_time] = ctrlCbfClfQp(obj, x, u_ref, with_slack, verbose) - %% Implementation of vanilla CBF-CLF-QP - % Inputs: x: state - % u_ref: reference control input - % with_slack: flag for relaxing the clf constraint(1: relax, 0: hard-constraint) - % verbose: flag for logging (1: print log, 0: run silently) - % Outputs: u: control input as a solution of the CBF-CLF-QP - % slack: slack variable for relaxation. (empty list when with_slack=0) - % B: Value of the CBF at current state. - % V: Value of the CLF at current state. - % feas: 1 if QP is feasible, 0 if infeasible. (Note: even - % when qp is infeasible, u is determined from quadprog.) - % comp_time: computation time to run the solver. +function [u, extraout] = ctrlCbfClfQp(obj, x, varargin) +%% [u, extraout] = ctrlCbfClfQp(obj, x, varagin) +%% Implementation of the vanilla CBF-CLF-QP +% Inputs: x: state +% varargin: +% u_ref: reference control input +% with_slack: flag for relaxing the clf constraint(1: relax, 0: hard-constraint) +% verbose: flag for logging (1: print log, 0: run silently) +% Outputs: u: control input as a solution of the CBF-CLF-QP +% extraout: +% slack: slack variable for relaxation. (empty when with_slack=0) +% Bs: CBF values at the current state. +% Vs: CLF values at the current state. +% feas: 1 if QP is feasible, 0 if infeasible. (Note: even +% when qp is infeasible, u is determined from quadprog.) +% comp_time: computation time to run the solver. +% Author: Jason Choi (jason.choi@berkeley.edu) if obj.n_clf == 0 error('CLF is not set up so ctrlCbfClfQp cannot be used.'); end @@ -19,16 +22,25 @@ error('CBF is not set up so ctrlCbfClfQp cannot be used.'); end - if nargin < 3 + kwargs = parse_function_args(varargin{:}); + if ~isfield(kwargs, 'u_ref') + % If u_ref is given, CLF-QP minimizes the norm of u-u_ref + % Default reference control input is u. u_ref = zeros(obj.udim, 1); + else + u_ref = kwargs.u_ref; end - if nargin < 4 + if ~isfield(kwargs, 'with_slack') + % Relaxing is activated in default condition. with_slack = 1; + else + with_slack = kwargs.with_slack; end - - if nargin < 5 + if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. verbose = 0; + else + verbose = kwargs.verbose; end if size(u_ref, 1) ~= obj.udim @@ -61,7 +73,7 @@ if with_slack % n_slack(size of slack): % = n_clf if n_cbf=1 (Relaxing only the CLF constraints) - % = (n_clf + n_cbf) if n_cbf >=1 (Relaxing all constraints) + % = (n_clf + n_cbf) if n_cbf >1 (Relaxing all constraints) if obj.n_cbf == 1 n_slack = obj.n_clf; A_slack = [-eye(obj.n_clf); zeros(1, obj.n_clf)]; @@ -77,18 +89,14 @@ end A = [A, A_slack]; end - - %% Cost - if isfield(obj.params.weight, 'input') - if size(obj.params.weight.input, 1) == 1 - weight_input = obj.params.weight.input * eye(obj.udim); - elseif all(size(obj.params.weight.input) == obj.udim) - weight_input = obj.params.weight.input; - else - error("params.weight.input should be either a scalar value or an (udim, udim) array.") - end + if ~isfield(kwargs, 'weight_slack') + weight_slack = obj.weight_slack * ones(n_slack, 1); else - weight_input = eye(obj.udim); + if numel(kwargs.weight_slack) ~= n_slack + error("wrong weight_slack size. it should be a vector of n_slack:=%s", ... + "n_clf if n_cbf==1, else (n_clf+n_cbf)."); + end + weight_slack = kwargs.weight_slack; end if verbose @@ -98,14 +106,15 @@ end if with_slack % cost = 0.5 [u' slack] H [u; slack] + f [u; slack] - % TODO: refactor handling obj.params.weight.slack - H = [weight_input, zeros(obj.udim, n_slack); - zeros(n_slack, obj.udim), diag(obj.params.weight.slack)]; - f_ = [-weight_input * u_ref; zeros(n_slack, 1)]; + H = [obj.weight_input, zeros(obj.udim, n_slack); + zeros(n_slack, obj.udim), diag(weight_slack)]; + f_ = [-obj.weight_input * u_ref; zeros(n_slack, 1)]; [u_slack, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + end u = zeros(obj.udim, 1); slack = zeros(n_slack, 1); else @@ -115,16 +124,24 @@ end else % cost = 0.5 u' H u + f u - H = weight_input; - f_ = -weight_input * u_ref; + H = obj.weight_input; + f_ = -obj.weight_input * u_ref; [u, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + end else feas = 1; end slack = []; end comp_time = toc(tstart); + + extraout.slack = slack; + extraout.Vs = Vs; + extraout.Bs = Bs; + extraout.feas = feas; + extraout.comp_time = comp_time; end \ No newline at end of file diff --git a/lib/@CtrlAffineSys/ctrlCbfQp.m b/lib/@CtrlAffineSys/ctrlCbfQp.m index e51e06c..8d8ba0e 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -1,29 +1,51 @@ -%% Author: Jason Choi (jason.choi@berkeley.edu) -function [u, slack, Bs, feas, comp_time] = ctrlCbfQp(obj, x, u_ref, verbose, with_slack) - %% Implementation of vanilla CBF-QP - % Inputs: x: state - % u_ref: reference control input - % verbose: flag for logging (1: print log, 0: run silently) - % Outputs: u: control input as a solution of the CBF-CLF-QP - % B: Value of the CBF at current state. - % feas: 1 if QP is feasible, 0 if infeasible. (Note: even - % when qp is infeasible, u is determined from quadprog.) - % compt_time: computation time to run the solver. +function [u, extraout] = ctrlCbfQp(obj, x, varargin) +%% [u, extraout] = ctrlCbfQp(obj, x, varagin) +%% Implementation of vanilla CBF-QP +% Inputs: x: state +% varargin: +% u_ref: reference control input +% verbose: flag for logging (1: print log, 0: run silently) +% Outputs: u: control input as a solution of the CBF-CLF-QP +% extraout: +% slack: slack variable for relaxation. (empty when with_slack=0) +% Bs: CBF values at current state. +% feas: 1 if QP is feasible, 0 if infeasible. (Note: even +% when qp is infeasible, u is determined from quadprog.) +% compt_time: computation time to run the solver. +% Author: Jason Choi (jason.choi@berkeley.edu) + if obj.n_cbf == 0 error('CBF is not set up so ctrlCbfQp cannot be used.'); end - if nargin < 3 + kwargs = parse_function_args(varargin{:}); + if ~isfield(kwargs, 'u_ref') + % If u_ref is given, CLF-QP minimizes the norm of u-u_ref + % Default reference control input is u. u_ref = zeros(obj.udim, 1); + else + u_ref = kwargs.u_ref; + end + if ~isfield(kwargs, 'with_slack') + % Relaxing is activated in default condition. + with_slack = 1; + else + with_slack = kwargs.with_slack; end - if nargin < 4 + if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. verbose = 0; + else + verbose = kwargs.verbose; end - if nargin < 5 - with_slack = obj.n_cbf > 1; + if ~isfield(kwargs, 'weight_slack') + weight_slack = obj.weight_slack * ones(obj.n_cbf); + else + if numel(kwargs.weight_slack) ~= obj.n_cbf + error("wrong weight_slack size. it should be a vector of length obj.n_cbf."); + end + weight_slack = kwargs.weight_slack; end - if size(u_ref, 1) ~= obj.udim error("Wrong size of u_ref, it should be (udim, 1) array."); end @@ -56,19 +78,6 @@ A = [A, A_slack]; end - %% Cost - if isfield(obj.params.weight, 'input') - if size(obj.params.weight.input, 1) == 1 - weight_input = obj.params.weight.input * eye(obj.udim); - elseif all(size(obj.params.weight.input) == obj.udim) - weight_input = obj.params.weight.input; - else - error("params.weight.input should be either a scalar value or an (udim, udim) array.") - end - else - weight_input = eye(obj.udim); - end - if verbose options = optimset('Display','notify'); else @@ -77,14 +86,23 @@ if with_slack % cost = 0.5 [u' slack] H [u; slack] + f [u; slack] - H = [weight_input, zeros(obj.udim, obj.n_cbf); - zeros(obj.n_cbf, obj.udim), diag(obj.params.weight.slack)]; - f_ = [-weight_input * u_ref; zeros(obj.n_cbf, 1)]; + H = [obj.weight_input, zeros(obj.udim, obj.n_cbf); + zeros(obj.n_cbf, obj.udim), diag(weight_slack)]; + f_ = [-obj.weight_input * u_ref; zeros(obj.n_cbf, 1)]; [u_slack, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. Numerical error might have occured."); + if verbose + disp("Infeasible QP. Numerical error might have occured."); + end u = zeros(obj.udim, 1); + % Making up best-effort heuristic solution, if single cbf + % constraint. + if obj.n_cbf == 1 + for i = 1:obj.udim + u(i) = obj.u_min(i) * (LgBs(i) <= 0) + obj.u_max(i) * (LgBs(i) > 0); + end + end slack = zeros(obj.n_clf, 1); else feas = 1; @@ -93,16 +111,23 @@ end else % cost = 0.5 u' H u + f u - H = weight_input; - f_ = -weight_input * u_ref; + H = obj.weight_input; + f_ = -obj.weight_input * u_ref; [u, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + end else feas = 1; end slack = []; end comp_time = toc(tstart); + + extraout.slack = slack; + extraout.Bs = Bs; + extraout.feas = feas; + extraout.comp_time = comp_time; end \ No newline at end of file diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index 18661d0..3f3eebb 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -1,33 +1,50 @@ -%% Author: Jason Choi (jason.choi@berkeley.edu) -function [u, slack, Vs, feas, comp_time] = ctrlClfQp(obj, x, u_ref, with_slack, verbose) - %% Implementation of vanilla CLF-QP - % Inputs: x: state - % u_ref: reference control input - % with_slack: flag for relaxing (1: relax, 0: hard CLF constraint) - % verbose: flag for logging (1: print log, 0: run silently) - % Outputs: u: control input as a solution of the CLF-QP - % slack: slack variable for relaxation. (empty list when with_slack=0) - % V: Value of the CLF at the current state. - % feas: 1 if QP is feasible, 0 if infeasible. (Note: even - % when qp is infeasible, u is determined from quadprog.) - % comp_time: computation time to run the solver. - +function [u, extraout] = ctrlClfQp(obj, x, varargin) +%% [u, extraout] = ctrlClfQp(obj, x, varagin) +%% Implementation of the vanilla CLF-QP +% Inputs: x: state +% varargin: +% u_ref: reference control input +% with_slack: flag for relaxing (1: relax, 0: hard CLF constraint) +% verbose: flag for logging (1: print log, 0: run silently) +% Outputs: u: control input as a solution of the CLF-QP +% extraout: +% slack: slack variable for relaxation. (empty when with_slack=0) +% Vs: CLF values at the current state. +% feas: 1 if QP is feasible, 0 if infeasible. (Note: even +% when qp is infeasible, u is determined from quadprog.) +% comp_time: computation time to run the solver. +% Author: Jason Choi (jason.choi@berkeley.edu) if obj.n_clf == 0 error('CLF is not set up so ctrlClfQp cannot be used.'); end - - if nargin < 3 || isempty(u_ref) + + kwargs = parse_function_args(varargin{:}); + if ~isfield(kwargs, 'u_ref') % If u_ref is given, CLF-QP minimizes the norm of u-u_ref % Default reference control input is u. u_ref = zeros(obj.udim, 1); + else + u_ref = kwargs.u_ref; end - if nargin < 4 + if ~isfield(kwargs, 'with_slack') % Relaxing is activated in default condition. with_slack = 1; + else + with_slack = kwargs.with_slack; end - if nargin < 5 + if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. verbose = 0; + else + verbose = kwargs.verbose; + end + if ~isfield(kwargs, 'weight_slack') + weight_slack = obj.weight_slack * ones(obj.n_clf); + else + if numel(kwargs.weight_slack) ~= obj.n_clf + error("wrong weight_slack size. it should be a vector of length obj.n_clf."); + end + weight_slack = kwargs.weight_slack; end if size(u_ref, 1) ~= obj.udim @@ -62,19 +79,6 @@ A = [A, A_slack]; end - %% Cost - if isfield(obj.params.weight, 'input') - if size(obj.params.weight.input, 1) == 1 - weight_input = obj.params.weight.input * eye(obj.udim); - elseif all(size(obj.params.weight.input) == obj.udim) - weight_input = obj.params.weight.input; - else - error("params.weight.input should be either a scalar value or an (udim, udim) array.") - end - else - weight_input = eye(obj.udim); - end - if verbose options = optimoptions('quadprog', 'ConstraintTolerance', 1e-6, 'StepTolerance', 1e-12, 'Display','iter'); else @@ -83,17 +87,22 @@ if with_slack % cost = 0.5 [u' slack] H [u; slack] + f [u; slack] - H = [weight_input, zeros(obj.udim, obj.n_clf); - zeros(obj.n_clf, obj.udim), diag(obj.params.weight.slack)]; - f_ = [-weight_input * u_ref; zeros(obj.n_clf, 1)]; + H = [obj.weight_input, zeros(obj.udim, obj.n_clf); + zeros(obj.n_clf, obj.udim), diag(weight_slack)]; + f_ = [-obj.weight_input * u_ref; zeros(obj.n_clf, 1)]; [u_slack, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. Numerical error might have occured."); - % Making up best-effort heuristic solution. + if verbose + disp("Infeasible QP. Numerical error might have occured."); + end u = zeros(obj.udim, 1); - for i = 1:obj.udim - u(i) = obj.u_min(i) * (LgVs(i) > 0) + obj.u_max(i) * (LgVs(i) <= 0); + % Making up best-effort heuristic solution, if single clf + % constraint. + if obj.n_clf == 1 + for i = 1:obj.udim + u(i) = obj.u_min(i) * (LgVs(i) > 0) + obj.u_max(i) * (LgVs(i) <= 0); + end end slack = zeros(obj.n_clf, 1); else @@ -102,12 +111,14 @@ slack = u_slack(obj.udim+1:end); end else - H = weight_input; - f_ = -weight_input * u_ref; + H = obj.weight_input; + f_ = -obj.weight_input * u_ref; [u, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CLF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. CLF constraint is conflicting with input constraints."); + end % Making up best-effort heuristic solution. u = zeros(obj.udim, 1); for i = 1:obj.udim @@ -119,4 +130,9 @@ slack = []; end comp_time = toc(tstart); + + extraout.slack = slack; + extraout.Vs = Vs; + extraout.feas = feas; + extraout.comp_time = comp_time; end diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 9d3e78a..b86e323 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -1,6 +1,10 @@ function init_sys(obj, params) +%% Functions that initilaize dynamic system +% Built in + % implant the params' parameter to object's parameters + % ex) x_dim, u_dim, n_clf, n_cbf, clf_rate, cbf_rate ... obj.params = params; - + if strcmp(obj.setup_option, 'symbolic') disp(['Setting up the dynamics, CLFs, CBFs from defined symbolic expressions.', ... '(This might take time.)']); @@ -16,11 +20,14 @@ function init_sys(obj, params) if ~isa(g_, 'sym') g_ = sym(g_); end - clf_ = obj.defineClf(params, x); - cbf_ = obj.defineCbf(params, x); % Setting state and input dimension. obj.xdim = size(x, 1); + obj.sdim = obj.xdim; % support past version, expedient obj.udim = size(g_, 2); + + clf_ = obj.defineClf(params, x); + cbf_ = obj.defineCbf(params, x); + obj.f_sym = matlabFunction(f_, 'vars', {x}); obj.g_sym = matlabFunction(g_, 'vars', {x}); @@ -86,10 +93,13 @@ function init_sys(obj, params) end elseif strcmp(obj.setup_option, 'built-in') + % extract param information and inject to object's property + % xdim, udim, n_clf, n_cbf if ~isfield(params, 'xdim') error("xdim should be specified for built-in setup."); end obj.xdim = params.xdim; + obj.sdim = obj.xdim; % support past version, expedient if ~isfield(params, 'udim') error("udim should be specified for built-in setup."); end @@ -97,14 +107,16 @@ function init_sys(obj, params) x_test = zeros(obj.xdim, 1); n_clf = 1; try - obj.clf(x_test); + clf_test = obj.clf(x_test); + n_clf = length(clf_test); catch e n_clf = 0; end obj.n_clf = n_clf; n_cbf = 1; try - obj.cbf(x_test); + cbf_test = obj.cbf(x_test); + n_cbf = length(cbf_test); catch e n_cbf = 0; end @@ -114,8 +126,8 @@ function init_sys(obj, params) end %% Parse parameters for both setup_option. + % set cbf rate, clf rate if isfield(params, 'clf') && isfield(params.clf, 'rate') - %% TODO: change this to params.clf_rate obj.clf_rate = params.clf.rate; elseif isfield(params, 'clf_rate') obj.clf_rate = params.clf_rate; @@ -124,7 +136,6 @@ function init_sys(obj, params) "in order to use CLF."); end if isfield(params, 'cbf') && isfield(params.cbf, 'rate') - %% TODO: change this to params.cbf_rate obj.cbf_rate = params.cbf.rate; elseif isfield(params, 'cbf_rate') obj.cbf_rate = params.cbf_rate; @@ -135,7 +146,7 @@ function init_sys(obj, params) if isfield(params, 'u_min') if length(params.u_min) == 1 obj.u_min = params.u_min * ones(obj.udim, 1); - elseif length(params.u_min) ~= obj.umin + elseif length(params.u_min) ~= obj.udim error("Invalid size of params.u_min."); else if isrow(params.u_min) @@ -148,7 +159,7 @@ function init_sys(obj, params) if isfield(params, 'u_max') if length(params.u_max) == 1 obj.u_max = params.u_max * ones(obj.udim, 1); - elseif length(params.u_max) ~= obj.umax + elseif length(params.u_max) ~= obj.udim error("Invalid size of params.u_max."); else if isrow(params.u_max) @@ -158,6 +169,73 @@ function init_sys(obj, params) end end end + %% Parse weight parameters. + % obj.weight_input is saved as (udim x udim) matrix. + if isfield(params, 'weight') + if isfield(params.weight, 'input') + weight_input = params.weight.input; + if length(weight_input) == 1 + obj.weight_input = weight_input * eye(obj.udim); + elseif isrow(weight_input) && length(weight_input) == obj.udim + obj.weight_input = diag(weight_input); + elseif iscolumn(params.weight.input) + obj.weight_input = diag(weight_input); + elseif all(size(obj.params.weight.input) == obj.udim) + obj.weight_input = weight_input; + else + error("params.weight.input should be either a scalar value%s", ... + "(udim) vector that contains the diagonal elements of ", ... + "the weight matrix, or the (udim, udim) full matrix.") + end + end + end + if isfield(params, 'weight_input') + weight_input = params.weight_input; + if length(weight_input) == 1 + obj.weight_input = weight_input * eye(obj.udim); + elseif isrow(weight_input) && length(weight_input) == obj.udim + obj.weight_input = diag(weight_input); + elseif iscolumn(params.weight.input) + obj.weight_input = diag(weight_input); + elseif all(size(obj.params.weight.input) == obj.udim) + obj.weight_input = weight_input; + else + error("params.weight_input should be either a scalar value%s", ... + "(udim) vector that contains the diagonal elements of ", ... + "the weight matrix, or the (udim, udim) full matrix.") + end + end + if isempty(obj.weight_input) + obj.weight_input = eye(obj.udim); + end + % obj.weight_slack is saved as a scalar value. + if isfield(params, 'weight') + if isfield(params.weight, 'slack') + weight_slack = params.weight.slack; + if length(weight_slack) == 1 + obj.weight_slack = weight_slack; + else + error("Default slack weight should be a scalar value. %s", ... + "If you want to specify different slack weights for ", ... + "each constraint, pass it to the controllers as ", ... + "additional argument 'weight_slack'"); + end + end + end + if isfield(params, 'weight_slack') + weight_slack = params.weight_slack; + if length(weight_slack) == 1 + obj.weight_slack = weight_slack; + else + error("Default slack weight should be a scalar value. %s", ... + "If you want to specify different slack weights for ", ... + "each constraint, pass it to the controllers as ", ... + "additional argument 'weight_slack'"); + end + end + if isempty(obj.weight_slack) + error("Either params.weight.slack or params.weight_slack should be provided."); + end %% Do sanity check if all necessary functions are set up properly. x_test = zeros(obj.xdim, 1); diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index 9e5963e..83e5b93 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -11,37 +11,53 @@ % Relative degree of y rel_deg_y % Output as a function handle (assuming relative degree 2) - y - phase + y_sym + phase_sym % 1st order Lie derivative of the output - lf_y - lg_y + lf_y_sym + lg_y_sym % 2nd order Lie derivatives of the output as function handles - lglf_y - l2f_y + lglf_y_sym + l2f_y_sym % output related functions needed handle phase bounds - y_max_exceed - lf_y_max_exceed - lg_y_max_exceed - lglf_y_max_exceed - l2f_y_max_exceed - y_min_exceed - lf_y_min_exceed - lg_y_min_exceed - lglf_y_min_exceed - l2f_y_min_exceed + y_max_exceed_sym + lf_y_max_exceed_sym + lg_y_max_exceed_sym + lglf_y_max_exceed_sym + l2f_y_max_exceed_sym + y_min_exceed_sym + lf_y_min_exceed_sym + lg_y_min_exceed_sym + lglf_y_min_exceed_sym + l2f_y_min_exceed_sym + % binary indicator on whether it uses phase-based output or not. + use_phase + + % rate of the RES-CLF. + eps_FL % CLF under feedback linearization and its derivatives as function handles. Gram_clf_FL % Gram matrix of the CLF for feedback linearization. + end methods - function obj = CtrlAffineSysFL(params) - obj@CtrlAffineSys(params); + function obj = CtrlAffineSysFL(params, setup_option) + if nargin < 1 + error("Warning: params argument is missing.") + end + disp(setup_option) + if nargin < 2 + setup_option = 'symbolic'; + end + if strcmp(setup_option, 'built_in') + setup_option = 'built-in'; + elseif strcmp(setup_option, 'builtin') + setup_option = 'built-in'; + end - [s, f, g] = defineSystem(obj, params); - [y, phase, y_max_exceed, y_min_exceed] = obj.defineOutput(params, s); - obj.initOutputDynamics(s, f, g, y, phase, y_max_exceed, y_min_exceed, params); + obj@CtrlAffineSys(params, setup_option); + obj.init_sys_FL(params); end function [y, phase, y_max_exceed, y_min_exceed] = defineOutput(obj, params, symbolic_state) @@ -52,24 +68,25 @@ end function V = clf_FL(obj, y, dy) - eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; + eta_eps = [(1/obj.eps_FL)*y; dy]; V = transpose(eta_eps)*obj.Gram_clf_FL*eta_eps; end function lF_clf_ = lF_clf_FL(obj, y, dy) - eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; + eta_eps = [(1/obj.eps_FL)*y; dy]; P = obj.Gram_clf_FL; lF_clf_ = transpose(eta_eps)*(transpose(obj.F_FL_eps)*P+P*obj.F_FL_eps)*eta_eps; end function lG_clf_ = lG_clf_FL(obj, y, dy) - eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; + eta_eps = [(1/obj.eps_FL)*y; dy]; P = obj.Gram_clf_FL; lG_clf_ = (2 * (obj.G_FL'*P) * eta_eps)'; end function [y, dy, L2fy, LgLfy, phase] = eval_y(obj, s) - if isempty(obj.phase) + % Todo: interpret this and make it more robust! + if ~obj.use_phase y = obj.y(s); dy = obj.lf_y(s); L2fy = obj.l2f_y(s); @@ -95,5 +112,174 @@ LgLfy = obj.lglf_y(s); end end + + %% Sym2Value Function + % Help convenient use of function handlers regardless of built-in + % and symbolic + % TODO: As relative degree increase, this way of coding would be + % inefficient. Other way to do this? + function y_ = y(obj, x) + % y_ = y(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the y = h(x) instead of defineOutput. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.y(x) should be overriden by user."); + end + y_ = obj.y_sym(x); + end + + function phase_ = phase(obj, x) + % phase_ = phase(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the phase + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.phase(x) should be overriden by user."); + end + phase_ = obj.phase_sym(x); + end + + function lf_y_ = lf_y(obj, x) + % lf_y_ = lf_y(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the + % L_f(y) + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lf_y(x) should be overriden by user."); + end + lf_y_ = obj.lf_y_sym(x); + end + + function lg_y_ = lg_y(obj, x) + % lg_y_ = lg_y(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the L_g(y). + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lg_y(x) should be overriden by user."); + end + lg_y_ = obj.lg_y_sym(x); + end + + function lglf_y_ = lglf_y(obj, x) + % lglf_y_ = lglf_y(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the L_g{L_f(y)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lglf_y(x) should be overriden by user."); + end + lglf_y_ = obj.lglf_y_sym(x); + end + + function l2f_y_ = l2f_y(obj, x) + % l2f_y_ = l2f_y(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the + % L2_f(y) + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.l2f_y(x) should be overriden by user."); + end + l2f_y_ = obj.l2f_y_sym(x); + end + + function y_max_exceed_ = y_max_exceed(obj, x) + % y_max_exceed_ = y_max_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the y_max_exceed(x) + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.y_max_exceed(x) should be overriden by user."); + end + y_max_exceed_ = obj.y_max_exceed_sym(x); + end + + function lf_y_max_exceed_ = lf_y_max_exceed(obj, x) + % lf_y_max_exceed_ = lf_y_max_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the + % L_f(y_max_exceed) + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lf_y_max_exceed(x) should be overriden by user."); + end + lf_y_max_exceed_ = obj.lf_y_max_exceed_sym(x); + end + + function lg_y_max_exceed_ = lg_y_max_exceed(obj, x) + % lg_y_max_exceed_ = lg_y_max_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the + % L_g(y_max_exceed) + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lg_y_max_exceed(x) should be overriden by user."); + end + lg_y_max_exceed_ = obj.lg_y_max_exceed_sym(x); + end + + function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) + % lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lglf_y_max_exceed(x) should be overriden by user."); + end + lglf_y_max_exceed_ = obj.lglf_y_max_exceed_sym(x); + end + + function l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) + % l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.l2f_y_max_exceed(x) should be overriden by user."); + end + l2f_y_max_exceed_ = obj.l2f_y_max_exceed_sym(x); + end + + function y_min_exceed_ = y_min_exceed(obj, x) + % y_min_exceed_ = y_min_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.y_min_exceed(x) should be overriden by user."); + end + y_min_exceed_ = obj.y_min_exceed_sym(x); + end + + function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) + % lf_y_min_exceed_ = lf_y_min_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lf_y_min_exceed(x) should be overriden by user."); + end + lf_y_min_exceed_ = obj.lf_y_min_exceed_sym(x); + end + + function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) + % lg_y_min_exceed_ = lg_y_min_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lg_y_min_exceed(x) should be overriden by user."); + end + lg_y_min_exceed_ = obj.lg_y_min_exceed_sym(x); + end + + function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) + % lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For 'built-in' setup_option, obj.lglf_y_min_exceed(x) should be overriden by user."); + end + lglf_y_min_exceed_ = obj.lglf_y_min_exceed_sym(x); + end + + function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) + % l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) + % For 'built-in' setup, override this function with the + % user-defined implementation of the lie derivative of the CBF L_g{B(x)}. + if strcmp(obj.setup_option, 'built-in') + error("For; 'built-in' setup_option, obj.l2f_y_min_exceed(x) should be overriden by user."); + end + l2f_y_min_exceed_ = obj.l2f_y_min_exceed_sym(x); + end end end diff --git a/lib/@CtrlAffineSysFL/ctrlCbfClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlCbfClfQpFL.m index ea082d1..0aead0c 100644 --- a/lib/@CtrlAffineSysFL/ctrlCbfClfQpFL.m +++ b/lib/@CtrlAffineSysFL/ctrlCbfClfQpFL.m @@ -1,5 +1,5 @@ %% Author: Jason Choi (jason.choi@berkeley.edu) -function [mu, slack, B, V, feas] = ctrlCbfClfQpFL(obj, s, mu_ref, with_slack, verbose) +function [mu, extraout] = ctrlCbfClfQpFL(obj, x, varargin) %% Implementation of CBF-CLF-QP under feedback linearization structure. % Inputs: s: state % mu_ref: reference virtual control input @@ -11,147 +11,136 @@ % V: Value of the CLF at current state. % feas: 1 if QP is feasible, 0 if infeasible. (Note: even % when qp is infeasible, u is determined from quadprog.) - if isempty(obj.cbf) - error('CBF is not defined so ctrlCbfClfQpFL cannot be used. Create a class function [defineCbf] and set up cbf with symbolic expression.'); - end - - if nargin < 3 + + % TODO: AffinesystemFL => u_ref, mu_ref (varargin input!) + kwargs = parse_function_args(varargin{:}); + if ~isfield(kwargs, 'mu_ref') + % If u_ref is given, CLF-QP minimizes the norm of u-u_ref + % Default reference control input is u. mu_ref = zeros(obj.udim, 1); + else + mu_ref = kwargs.mu_ref; end - if nargin < 4 + if ~isfield(kwargs, 'with_slack') + % Relaxing is activated in default condition. with_slack = 1; + else + with_slack = kwargs.with_slack; end - if nargin < 5 + if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. verbose = 0; + else + verbose = kwargs.verbose; end if size(mu_ref, 1) ~= obj.udim error("Wrong size of mu_ref, it should be (udim, 1) array."); - end + end - V = obj.clf_FL(s); - LfV = obj.lF_clf_FL(s); - LgV = obj.lG_clf_FL(s); + tstart = tic; + [y, dy, L2fy, LgLfy, ~] = obj.eval_y(x); + + V = obj.clf_FL(y, dy); + LfV = obj.lF_clf_FL(y, dy); + LgV = obj.lG_clf_FL(y, dy); - B = obj.cbf(s); - LfB = obj.lf_cbf(s); - LgB = obj.lg_cbf(s); + Bs = obj.cbf(x); + LfBs = obj.lf_cbf(x); + LgBs = obj.lg_cbf(x); - y = obj.y(s); - L2fy = obj.l2f_y(s); - LgLfy = obj.lglf_y(s); inv_LgLfy = inv(LgLfy); u_star = -LgLfy\L2fy; %feedforward term + + + %% Constraint : A[u; slack] <= b + A = [LgV; -LgBs]; + b = [-LfV - obj.clf_rate * V; + LfBs + obj.cbf_rate * Bs]; + if ~isempty(obj.u_max) + A = [A; inv_LgLfy]; + b = [b; obj.u_max - u_star]; + end + if ~isempty(obj.u_min) + A = [A; -inv_LgLfy]; + b = [b; u_star - obj.u_min]; + end if with_slack - %% Decision variables (obj.udim + 2) - %% 1-udim: \mu - %% udim+1: virtual cbf input (lie derivative) - %% udim+2: slack - A = [LgV, 0, -1; - zeros(1, obj.udim), -1, 0]; - b = [-LfV - obj.params.clf.rate * V; - obj.params.cbf.rate * B]; - Aeq = [LgB * inv_LgLfy, -1, 0]; - beq = [-LfB-LgB*u_star]; - %% Add input constraints if u_max or u_min exists. - if isfield(obj.params, 'u_max') - A = [A; inv_LgLfy, zeros(obj.udim, 2);]; - if size(obj.params.u_max, 1) == 1 - b = [b; obj.params.u_max * ones(obj.udim, 1)-u_star]; - elseif size(obj.params.u_max, 1) == obj.udim - b = [b; obj.params.u_max-ustar]; - else - error("params.u_max should be either a scalar value or an (udim, 1) array.") - end + % n_slack(size of slack): + % = n_clf if n_cbf=1 (Relaxing only the CLF constraints) + % = (n_clf + n_cbf) if n_cbf >1 (Relaxing all constraints) + if obj.n_cbf == 1 + n_slack = obj.n_clf; + A_slack = [-eye(obj.n_clf); zeros(1, obj.n_clf)]; + else + n_slack = obj.n_clf + obj.n_cbf; + A_slack = -eye(obj.n_clf + obj.n_cbf); end - if isfield(obj.params, 'u_min') - A = [A; -inv_LgLfy, zeros(obj.udim, 2);]; - if size(obj.params.u_min, 1) == 1 - b = [b; u_star-obj.params.u_min * ones(obj.udim, 1)]; - elseif size(obj.params.u_min, 1) == obj.udim - b = [b; u_star-obj.params.u_min]; - else - error("params.u_min should be either a scalar value or an (udim, 1) array") - end - end - else - %% Decision variables (obj.udim + 1) - %% 1-udim: \mu - %% udim+1: virtual cbf input (lie derivative) - A = [LgV, 0; - zeros(1, obj.udim), -1]; - b = [-LfV - obj.params.clf.rate * V; - obj.params.cbf.rate * B]; - Aeq = [LgB * inv_LgLfy, -1]; - beq = [-LfB-LgB*u_star]; - %% Add input constraints if u_max or u_min exists. - if isfield(obj.params, 'u_max') - A = [A; inv_LgLfy, zeros(obj.udim, 1);]; - if size(obj.params.u_max, 1) == 1 - b = [b; obj.params.u_max * ones(obj.udim, 1)-u_star]; - elseif size(obj.params.u_max, 1) == obj.udim - b = [b; obj.params.u_max-ustar]; - else - error("params.u_max should be either a scalar value or an (udim, 1) array.") - end + if ~isempty(obj.u_max) + A_slack = [A_slack; zeros(obj.udim, n_slack)]; end - if isfield(obj.params, 'u_min') - A = [A; -inv_LgLfy, zeros(obj.udim, 1);]; - if size(obj.params.u_min, 1) == 1 - b = [b; u_star-obj.params.u_min * ones(obj.udim, 1)]; - elseif size(obj.params.u_min, 1) == obj.udim - b = [b; u_star-obj.params.u_min]; - else - error("params.u_min should be either a scalar value or an (udim, 1) array") - end + if ~isempty(obj.u_min) + A_slack = [A_slack; zeros(obj.udim, n_slack)]; end + A = [A, A_slack]; end - - - %% Cost - if isfield(obj.params.weight, 'input') - if size(obj.params.weight.input, 1) == 1 - weight_input = obj.params.weight.input * eye(obj.udim); - elseif all(size(obj.params.weight.input) == obj.udim) - weight_input = obj.params.weight.input; - else - error("params.weight.input should be either a scalar value or an (udim, udim) array.") - end + + if ~isfield(kwargs, 'weight_slack') + weight_slack = obj.weight_slack * ones(n_slack, 1); else - weight_input = eye(obj.udim); + if numel(kwargs.weight_slack) ~= n_slack + error("wrong weight_slack size. it should be a vector of n_slack:=%s", ... + "n_clf if n_cbf==1, else (n_clf+n_cbf)."); + end + weight_slack = kwargs.weight_slack; end + if verbose options = optimset('Display','notify'); else options = optimset('Display','off'); end + if with_slack % cost = 0.5 [u' slack] H [u; slack] + f [u; slack] - H = [weight_input, zeros(obj.udim, 1); - zeros(1, obj.udim), obj.params.weight.slack]; - f_ = [-weight_input * mu_ref; 0]; - [mu_slack, ~, exitflag, ~] = quadprog(H, f_, A, b, Aeq, beq, [], [], [], options); + H = [obj.weight_input, zeros(obj.udim, n_slack); + zeros(n_slack, obj.udim), diag(weight_slack)]; + f_ = [-obj.weight_input * mu_ref; zeros(n_slack, 1)]; + [mu_slack, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. Numerical error might have occured."); + end + mu = zeros(obj.udim ,1); + slack = 0; else feas = 1; + mu = mu_slack(1:obj.udim); + slack = mu_slack(end); end - mu = mu_slack(1:obj.udim); - slack = mu_slack(end); else % cost = 0.5 u' H u + f u - H = weight_input; - f_ = -weight_input * mu_ref; - [mu, ~, exitflag, ~] = quadprog(H, f_, A, b, Aeq, beq, [], [], [], options); + H = obj.weight_input; + f_ = -obj.weight_input * mu_ref; + [mu, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. constraints are conflicting with input constraints."); + end + mu = zeros(obj.udim, 1); else feas = 1; end slack = []; end + comp_time = toc(tstart); + + extraout.slack = slack; + extraout.feas = feas; + extraout.Vs = V; + extraout.Bs = Bs; + extraout.comp_time = comp_time; end \ No newline at end of file diff --git a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m index abdac78..f771773 100644 --- a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m +++ b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m @@ -1,6 +1,6 @@ %% Author: Jason Choi (jason.choi@berkeley.edu) -function [mu, slack, B, V, feas] = ctrlClfQpFL(obj, s, mu_ref, with_slack, verbose) - %% Implementation of CBF-CLF-QP under feedback linearization structure. +function [mu, extraout] = ctrlClfQpFL(obj, x, varargin) + %% Implementation of CLF-QP under feedback linearization structure. % Inputs: s: state % mu_ref: reference virtual control input % with_slack: flag for relaxing the clf constraint(1: relax, 0: hard-constraint) @@ -10,23 +10,42 @@ % B: Value of the CBF at current state. % V: Value of the CLF at current state. % feas: 1 if QP is feasible, 0 if infeasible. (Note: even - % when qp is infeasible, u is determined from quadprog.) - if nargin < 3 + % when qp is infeasible, u is determined from quadprog.) + + % TODO: AffinesystemFL => u_ref, mu_ref (varargin input!) + kwargs = parse_function_args(varargin{:}); + if ~isfield(kwargs, 'mu_ref') + % If u_ref is given, CLF-QP minimizes the norm of u-u_ref + % Default reference control input is u. mu_ref = zeros(obj.udim, 1); + else + mu_ref = kwargs.mu_ref; end - if nargin < 4 + if ~isfield(kwargs, 'with_slack') + % Relaxing is activated in default condition. with_slack = 1; + else + with_slack = kwargs.with_slack; end - if nargin < 5 + if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. verbose = 0; + else + verbose = kwargs.verbose; + end + + if ~isfield(kwargs, 'weight_slack') + weight_slack = obj.weight_slack; + else + weight_slack = kwargs.weight_slack; end if size(mu_ref, 1) ~= obj.udim error("Wrong size of mu_ref, it should be (udim, 1) array."); end - [y, dy, L2fy, LgLfy, phase] = obj.eval_y(s); + tstart = tic; + [y, dy, L2fy, LgLfy, ~] = obj.eval_y(x); V = obj.clf_FL(y, dy); LfV = obj.lF_clf_FL(y, dy); @@ -34,73 +53,27 @@ inv_LgLfy = inv(LgLfy); u_star = -LgLfy\L2fy; %feedforward term + + %% Constraints : A[u; slack] <= b + A = LgV; + b = -LfV - obj.clf_rate * V; + if ~isempty(obj.u_max) + A = [A; inv_LgLfy]; + b = [b; obj.u_max - u_star]; + end + if ~isempty(obj.u_min) + A = [A; -inv_LgLfy]; + b = [b; u_star - obj.u_min]; + end if with_slack - %% Decision variables (obj.udim + 1) - %% 1-udim: \mu - %% udim+1: slack - A = [LgV, -1]; - b = [-LfV - obj.params.clf.rate * V]; - %% Add input constraints if u_max or u_min exists. - if isfield(obj.params, 'u_max') - A = [A; inv_LgLfy, zeros(obj.udim, 1);]; - if size(obj.params.u_max, 1) == 1 - b = [b; obj.params.u_max * ones(obj.udim, 1)-u_star]; - elseif size(obj.params.u_max, 1) == obj.udim - b = [b; obj.params.u_max-ustar]; - else - error("params.u_max should be either a scalar value or an (udim, 1) array.") - end - end - if isfield(obj.params, 'u_min') - A = [A; -inv_LgLfy, zeros(obj.udim, 1);]; - if size(obj.params.u_min, 1) == 1 - b = [b; u_star-obj.params.u_min * ones(obj.udim, 1)]; - elseif size(obj.params.u_min, 1) == obj.udim - b = [b; u_star-obj.params.u_min]; - else - error("params.u_min should be either a scalar value or an (udim, 1) array") - end - end - else - %% Decision variables (obj.udim) - %% 1-udim: \mu - A = [LgV]; - b = [-LfV - obj.params.clf.rate * V]; - %% Add input constraints if u_max or u_min exists. - if isfield(obj.params, 'u_max') - A = [A; inv_LgLfy]; - if size(obj.params.u_max, 1) == 1 - b = [b; obj.params.u_max * ones(obj.udim, 1)-u_star]; - elseif size(obj.params.u_max, 1) == obj.udim - b = [b; obj.params.u_max-ustar]; - else - error("params.u_max should be either a scalar value or an (udim, 1) array.") - end - end - if isfield(obj.params, 'u_min') - A = [A; -inv_LgLfy]; - if size(obj.params.u_min, 1) == 1 - b = [b; u_star-obj.params.u_min * ones(obj.udim, 1)]; - elseif size(obj.params.u_min, 1) == obj.udim - b = [b; u_star-obj.params.u_min]; - else - error("params.u_min should be either a scalar value or an (udim, 1) array") - end + A_slack = -1; + if ~isempty(obj.u_max) + A_slack = [A_slack; zeros(obj.udim, 1)]; end - end - - - %% Cost - if isfield(obj.params.weight, 'input') - if size(obj.params.weight.input, 1) == 1 - weight_input = obj.params.weight.input * eye(obj.udim); - elseif all(size(obj.params.weight.input) == obj.udim) - weight_input = obj.params.weight.input; - else - error("params.weight.input should be either a scalar value or an (udim, udim) array.") + if ~isempty(obj.u_min) + A_slack = [A_slack; zeros(obj.udim, 1)]; end - else - weight_input = eye(obj.udim); + A = [A, A_slack]; end if verbose @@ -108,31 +81,48 @@ else options = optimset('Display','off'); end - if with_slack + + if with_slack % cost = 0.5 [u' slack] H [u; slack] + f [u; slack] - H = [weight_input, zeros(obj.udim, 1); - zeros(1, obj.udim), obj.params.weight.slack]; - f_ = [-weight_input * mu_ref; 0]; + H = [obj.weight_input, zeros(obj.udim, 1); + zeros(1, obj.udim), weight_slack]; + f_ = [-obj.weight_input * mu_ref; 0]; + [mu_slack, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. Numerical error might have occured."); + end + %% TODO: think about the backup controller. (FYI: ctrlClfQp.m) + mu = zeros(obj.udim, 1); + slack = 0; else feas = 1; + mu = mu_slack(1:obj.udim); + slack = mu_slack(end); end - mu = mu_slack(1:obj.udim); - slack = mu_slack(end); else % cost = 0.5 u' H u + f u - H = weight_input; - f_ = -weight_input * mu_ref; + H = obj.weight_input; + f_ = -obj.weight_input * mu_ref; [mu, ~, exitflag, ~] = quadprog(H, f_, A, b, [], [], [], [], [], options); if exitflag == -2 feas = 0; - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + if verbose + disp("Infeasible QP. CLF constraint is conflicting with input constraints."); + end + %% TODO: think about the backup controller. (FYI: ctrlClfQp.m) + mu = zeros(obj.udim, 1); else feas = 1; end slack = []; end + comp_time = toc(tstart); + + extraout.slack = slack; + extraout.feas = feas; + extraout.Vs = V; + extraout.comp_time = comp_time; end \ No newline at end of file diff --git a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m index 383c519..346064c 100644 --- a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m +++ b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m @@ -1,65 +1,83 @@ -function [u, y, feedforward, u_raw] = ctrlFeedbackLinearize(obj, s, mu) +function [u, extraout] = ctrlFeedbackLinearize(obj, x, mu, varargin) +%% [u, extraout] = ctrlFeedbackLinearize(obj, x, mu, varargin) %% Implementation of feedback linearization structure -% Inputs: s: state +% Inputs: x: state % mu: virtual control input +% mu can be a vector of (obj.udim, 1) or it can also be a +% function handle. +% varargin: extra inputs (only necessary when mu is a handle.) % Outputs: u: control input. -y = obj.y(s); +% extraout: y: output (defined for feedback linearization). +% feedforward: feedforward term of the control +% u_raw: control input before clipped by the input saturation +% constraint. +% When mu is a function handle, it might contain other fields +% that are from the controller that defines mu. +% u = u_star + LgLf\mu +kwargs = parse_function_args(varargin{:}); +if ~isfield(kwargs, 'verbose') + % Run QP without log in default condition. + verbose = 0; +else + verbose = kwargs.verbose; +end + +if isa(mu, 'function_handle') + [mu_, extraout] = mu(x, varargin{:}); +elseif isa(mu, 'numeric') + mu_ = mu; +else + error("Unknown mu type. mu should be either the numeric vector or %s", ... + "the function handle that defines the controller.") +end + +[y, dy, ~, ~, ~] = obj.eval_y(x); if obj.rel_deg_y == 1 - Lfy = obj.lf_y(s); - Lgy = obj.lg_y(s); + Lfy = obj.lf_y(x); + Lgy = obj.lg_y(x); if rank(Lgy) < obj.ydim error("Feedback Linearization fail because lg_y is not invertible") end feedforward = -Lgy\Lfy; - u_raw = feedforward + Lgy\mu; + u_raw = feedforward + Lgy\mu_; elseif obj.rel_deg_y == 2 - LgLfy = obj.lglf_y(s); - L2f_y = obj.l2f_y(s); + LgLfy = obj.lglf_y(x); + L2f_y = obj.l2f_y(x); if rank(LgLfy) < obj.ydim error("Feedback Linearization fail because lglf_y is not invertible") end feedforward = -LgLfy\L2f_y; - u_raw = feedforward + LgLfy\mu; + u_raw = feedforward + LgLfy\mu_; end u = u_raw; -if isfield(obj.params, 'u_max') - if size(obj.params.u_max, 1) == 1 - for i = 1:obj.udim - if u(i) > obj.params.u_max - u(i) = obj.params.u_max; + +%% Clip input +if ~isempty(obj.u_max) + for i = 1:obj.udim + if u(i) > obj.u_max(i) + if verbose disp("Warning: virtual control input does not satisfy input constraint.") end - end - elseif size(obj.params.u_max, 1) == obj.udim - for i = 1:obj.udim - if u(i) > obj.params.u_max(i) - u(i) = obj.params.u_max(i); - disp("Warning: virtual control input does not satisfy input constraint.") - end - end - else - error("params.u_max Unknown size.") - end + u(i) = obj.u_max(i); + end + end end -if isfield(obj.params, 'u_min') - if size(obj.params.u_min, 1) == 1 - for i = 1:obj.udim - if u(i) < obj.params.u_min - u(i) = obj.params.u_min; +if ~isempty(obj.u_min) + for i = 1:obj.udim + if u(i) < obj.u_min(i) + if verbose disp("Warning: virtual control input does not satisfy input constraint.") end - end - elseif size(obj.params.u_min, 1) == obj.udim - for i = 1:obj.udim - if u(i) < obj.params.u_min(i) - u(i) = obj.params.u_min(i); - disp("Warning: virtual control input does not satisfy input constraint.") - end - end - else - error("params.u_min Unknown size.") - end -end \ No newline at end of file + u(i) = obj.u_min(i); + end + end +end + +extraout.mu = mu_; +extraout.y = y; +extraout.dy = dy; +extraout.feedforward = feedforward; +extraout.u_raw = u_raw; \ No newline at end of file diff --git a/lib/@CtrlAffineSysFL/initOutputDynamics.m b/lib/@CtrlAffineSysFL/initOutputDynamics.m index 2a7bf9c..c41ba16 100644 --- a/lib/@CtrlAffineSysFL/initOutputDynamics.m +++ b/lib/@CtrlAffineSysFL/initOutputDynamics.m @@ -1,4 +1,13 @@ function initOutputDynamics(obj, symbolic_s, symbolic_f, symbolic_g, symbolic_y, symbolic_phase, symbolic_y_max_exceed, symbolic_y_min_exceed, params) +% Construct essential property of IO-FL, prepare for CLF +% ex) F, G, F_epsilon, P, Q, A, gamma +% From +% params => epsilon_FL, Q_F, Kd_FL, Kp_FL +% To +% object => ydim, udim, y, rel_deg_y, lf_y, lg_y, l2f_y, lglf_y +% => y_exceed_max, lf_y_exceed_max, lg_y_exceed_max, +% l2f_y_exceed_max, lglf_y_exeed_max as a function +% => F_FL, G_FL, Gram_CLF_FL(P), F_FL_eps, clf_rate if isempty(symbolic_s) || isempty(symbolic_f) || isempty(symbolic_g) error('s, f, g is empty. Create a class function defineSystem and define your dynamics with symbolic expression.'); end @@ -24,12 +33,12 @@ function initOutputDynamics(obj, symbolic_s, symbolic_f, symbolic_g, symbolic_y, dy = simplify(jacobian(symbolic_y, symbolic_s)); lf_y_ = dy * f_; lg_y_ = simplify(dy * g_); - if ~isAlways(norm(lg_y_) == 0) + if ~isAlways(norm(lg_y_) == 0) % ||Lgy||=0, relative degree is 1 obj.rel_deg_y = 1; disp("WARNING: output relative degree 1. Currently, the CLF for feedback linearization only supports 2."); - obj.y = matlabFunction(symbolic_y, 'vars', {s}); - obj.lf_y = matlabFunction(lf_y_, 'vars', {s}); - obj.lg_y = matlabFunction(lg_y_, 'vars', {s}); + obj.y_sym = matlabFunction(symbolic_y, 'vars', {s}); + obj.lf_y_sym = matlabFunction(lf_y_, 'vars', {s}); + obj.lg_y_sym = matlabFunction(lg_y_, 'vars', {s}); else dlf_y_ = simplify(jacobian(lf_y_, symbolic_s)); l2f_y_ = dlf_y_ * f_; @@ -38,80 +47,80 @@ function initOutputDynamics(obj, symbolic_s, symbolic_f, symbolic_g, symbolic_y, error("output relative degree is higher than 2, this is currently not supported by the library."); end obj.rel_deg_y = 2; - obj.y = matlabFunction(symbolic_y, 'vars', {s}); - obj.lf_y = matlabFunction(lf_y_, 'vars', {s}); - obj.l2f_y = matlabFunction(l2f_y_, 'vars', {s}); - obj.lglf_y = matlabFunction(lglf_y_, 'vars', {s}); + obj.y_sym = matlabFunction(symbolic_y, 'vars', {s}); + obj.lf_y_sym = matlabFunction(lf_y_, 'vars', {s}); + obj.l2f_y_sym = matlabFunction(l2f_y_, 'vars', {s}); + obj.lglf_y_sym = matlabFunction(lglf_y_, 'vars', {s}); %% Phase related if ~isempty(symbolic_phase) - obj.phase = matlabFunction(symbolic_phase, 'vars', {s}); + obj.phase_sym = matlabFunction(symbolic_phase, 'vars', {s}); % y to use when phase exceed max bound. dy_max_exceed = simplify(jacobian(symbolic_y_max_exceed, symbolic_s)); lf_y_max_exceed_ = dy_max_exceed * f_; dlf_y_max_exceed_ = jacobian(lf_y_max_exceed_, symbolic_s); l2f_y_max_exceed_ = dlf_y_max_exceed_ * f_; lglf_y_max_exceed_ = dlf_y_max_exceed_ * g_; - obj.y_max_exceed = matlabFunction(symbolic_y_max_exceed, 'vars', {s}); - obj.lf_y_max_exceed = matlabFunction(lf_y_max_exceed_, 'vars', {s}); - obj.l2f_y_max_exceed = matlabFunction(l2f_y_max_exceed_, 'vars', {s}); - obj.lglf_y_max_exceed = matlabFunction(lglf_y_max_exceed_, 'vars', {s}); - obj.phase = matlabFunction(symbolic_phase, 'vars', {s}); + obj.y_max_exceed_sym = matlabFunction(symbolic_y_max_exceed, 'vars', {s}); + obj.lf_y_max_exceed_sym = matlabFunction(lf_y_max_exceed_, 'vars', {s}); + obj.l2f_y_max_exceed_sym = matlabFunction(l2f_y_max_exceed_, 'vars', {s}); + obj.lglf_y_max_exceed_sym = matlabFunction(lglf_y_max_exceed_, 'vars', {s}); + obj.phase_sym = matlabFunction(symbolic_phase, 'vars', {s}); % y to use when phase exceed min bound. dy_min_exceed = simplify(jacobian(symbolic_y_min_exceed, symbolic_s)); lf_y_min_exceed_ = dy_min_exceed * f_; dlf_y_min_exceed_ = jacobian(lf_y_min_exceed_, symbolic_s); l2f_y_min_exceed_ = dlf_y_min_exceed_ * f_; lglf_y_min_exceed_ = dlf_y_min_exceed_ * g_; - obj.y_min_exceed = matlabFunction(symbolic_y_min_exceed, 'vars', {s}); - obj.lf_y_min_exceed = matlabFunction(lf_y_min_exceed_, 'vars', {s}); - obj.l2f_y_min_exceed = matlabFunction(l2f_y_min_exceed_, 'vars', {s}); - obj.lglf_y_min_exceed = matlabFunction(lglf_y_min_exceed_, 'vars', {s}); + obj.y_min_exceed_sym = matlabFunction(symbolic_y_min_exceed, 'vars', {s}); + obj.lf_y_min_exceed_sym = matlabFunction(lf_y_min_exceed_, 'vars', {s}); + obj.l2f_y_min_exceed_sym = matlabFunction(l2f_y_min_exceed_, 'vars', {s}); + obj.lglf_y_min_exceed_sym = matlabFunction(lglf_y_min_exceed_, 'vars', {s}); end - %% Set up desired linear output dynamics. - if ~isfield(params, 'epsilon_FL') - error("Define variable in the params called 'epsilon_FL', which is the rate of RES-CLF"); - end - eps = params.epsilon_FL; - obj.F_FL = [zeros(obj.ydim), eye(obj.ydim); - zeros(obj.ydim), zeros(obj.ydim)]; - obj.F_FL_eps = [zeros(obj.ydim), 1/eps * eye(obj.ydim); - zeros(obj.ydim), zeros(obj.ydim)]; - obj.G_FL = [zeros(obj.ydim); - eye(obj.ydim)]; - - %% Set up CLF. - if ~isfield(params, 'Kp_FL') - error("Define variable in the params called 'Kp_FL', p gain for RES-CLF"); - end - if ~isfield(params, 'Kd_FL') - error("Define variable in the params called 'Kd_FL', d gain for RES-CLF"); - end - if ~isfield(params, 'Q_FL') - disp("Setting Q matrix for CLF to default identity matrix."); - Q = eye(2*obj.ydim); - else - if size(params.Q_FL, 1) ~= 2*obj.ydim - error("Wrong Q_FL size.") - end - Q = params.Q_FL; - end - Kp = params.Kp_FL; - Kd = params.Kd_FL; - A = [zeros(obj.ydim), eye(obj.ydim); - -Kp*eye(obj.ydim), -Kd*eye(obj.ydim)]; - obj.Gram_clf_FL = lyap(A', Q); - - if isfield(params, 'clf') - if ~isfield(params.clf, 'rate') - obj.params.clf.rate = min(eig(Q))/max(eig(obj.Gram_clf_FL))/eps; - fprintf("Setting clf rate automatically to %.3f \n", obj.params.clf.rate); - end - else - obj.params.clf.rate = min(eig(Q))/max(eig(obj.Gram_clf_FL))/eps; - fprintf("Setting clf rate automatically to %.3f \n", obj.params.clf.rate); - end +% %% Set up desired linear output dynamics. +% if ~isfield(params, 'epsilon_FL') +% error("Define variable in the params called 'epsilon_FL', which is the rate of RES-CLF"); +% end +% eps = params.epsilon_FL; +% obj.F_FL = [zeros(obj.ydim), eye(obj.ydim); +% zeros(obj.ydim), zeros(obj.ydim)]; +% obj.F_FL_eps = [zeros(obj.ydim), 1/eps * eye(obj.ydim); +% zeros(obj.ydim), zeros(obj.ydim)]; +% obj.G_FL = [zeros(obj.ydim); +% eye(obj.ydim)]; +% +% %% Set up CLF. +% if ~isfield(params, 'Kp_FL') +% error("Define variable in the params called 'Kp_FL', p gain for RES-CLF"); +% end +% if ~isfield(params, 'Kd_FL') +% error("Define variable in the params called 'Kd_FL', d gain for RES-CLF"); +% end +% if ~isfield(params, 'Q_FL') +% disp("Setting Q matrix for CLF to default identity matrix."); +% Q = eye(2*obj.ydim); +% else +% if size(params.Q_FL, 1) ~= 2*obj.ydim +% error("Wrong Q_FL size.") +% end +% Q = params.Q_FL; +% end +% Kp = params.Kp_FL; +% Kd = params.Kd_FL; +% A = [zeros(obj.ydim), eye(obj.ydim); +% -Kp*eye(obj.ydim), -Kd*eye(obj.ydim)]; +% obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P +% +% if isfield(params, 'clf') +% if ~isfield(params.clf, 'rate') +% obj.params.clf.rate = min(eig(Q))/max(eig(obj.Gram_clf_FL))/eps; +% fprintf("Setting clf rate automatically to %.3f \n", obj.params.clf.rate); +% end +% else +% obj.params.clf.rate = min(eig(Q))/max(eig(obj.Gram_clf_FL))/eps; +% fprintf("Setting clf rate automatically to %.3f \n", obj.params.clf.rate); +% end % obj.clf_FL = @(y, dy) control_lyapunov_function(y, dy, obj, eps, P); % obj.lF_clf_FL = @(y, dy) lF_clf(y, dy, obj, eps, P); diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m new file mode 100644 index 0000000..5ccc668 --- /dev/null +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -0,0 +1,160 @@ +function init_sys_FL(obj, params) +%% Functions that initialize dynamic system + if ~isfield(params, 'use_phase') + obj.use_phase = 0; + else + obj.use_phase = params.use_phase; + obj.params = rmfield(obj.params, 'use_phase'); + end + if obj.use_phase + error("Currently not supported."); + end + + %% Symbolic Function + if strcmp(obj.setup_option, 'symbolic') + disp(['Setting up feedback linearization dynamics, CLFs, CBFs from defined symbolic expressions.', ... + '(This might take time.)']); + [x, f, g] = obj.defineSystem(params); + [y, phase, y_max_exceed, y_min_exceed] = obj.defineOutput(params, x); + obj.initOutputDynamics(x, f, g, y, phase, y_max_exceed, y_min_exceed, params); + + %% Non Symbolic Function + elseif strcmp(obj.setup_option, 'built-in') + if ~isfield(params, 'rel_deg_y') + error("rel_deg_y should be specified for built-in setup."); + end + obj.rel_deg_y = params.rel_deg_y; % TODO: other way to judge it? + if obj.rel_deg_y ~= 2 + error("Not Suppported"); + end + obj.ydim = obj.udim; + else + error("Undefined setup_option."); + end + %% Set up desired linear output dynamics + %% Both Symbolic and Non-symbolic + if ~isfield(params, 'epsilon_FL') && ~isfield(params, 'eps_FL') + disp(["[Warning] The rate of RES-CLF, epsilon_FL, is set to a default value 1", ... + "It is strongly recommended to use a custom value (which can be specified by params.epsilon_FL)."]); + eps = 1; + elseif isfield(params, 'epsilon_FL') + eps = params.epsilon_FL; + obj.params = rmfield(obj.params, 'epsilon_FL'); + elseif isfield(params, 'eps_FL') + eps = params.eps_FL; + obj.params = rmfield(obj.params, 'eps_FL'); + end + obj.eps_FL = eps; + obj.F_FL = [zeros(obj.ydim), eye(obj.ydim); + zeros(obj.ydim), zeros(obj.ydim)]; + obj.F_FL_eps = [zeros(obj.ydim), 1/eps * eye(obj.ydim); + zeros(obj.ydim), zeros(obj.ydim)]; + obj.G_FL = [zeros(obj.ydim); + eye(obj.ydim)]; + + %% Set up CLF. + % In controlSysFL, CLF is automatically set, so obj.n_clf can be + % hard-coded to 1 + obj.n_clf = 1; + %% There are three ways to specify the CLF. + % Option 1. Directly specify params.P_FL, the gram matrix of the Lyapunov + % function. + % Option 2. Specify params.Q_FL, params.Kp_FL, params.Ld_FL, the + % feedback gains, and sovle the lyapunov equation for the closed loop + % system. + % Option 3. Specify params.Q_FL, params.R_FL, the weight matrices in the LQR. + if isfield(params, 'P_FL') || isfield(params, 'Gram_clf_FL') ... + || (isfield(params, 'clf') && isfield(params.clf, 'P')) + clf_type = 1; + elseif isfield(params, 'Kp_FL') || isfield(params, 'Kd_FL') + clf_type = 2; + else + clf_type = 3; + end + + if clf_type == 1 + if isfield(params, 'P_FL') + if size(params.P_FL, 1) ~= 2*obj.ydim || size(params.P_FL, 2) ~= 2 * obj.ydim + error("Wrong params.P_FL size.") + end + obj.Gram_clf_FL = params.P_FL; + obj.params = rmfield(obj.params, 'P_FL'); + elseif isfield(params, 'Gram_clf_FL') + if size(params.Gram_clf_FL, 1) ~= 2*obj.ydim ... + || size(params.Gram_clf_FL, 2) ~= 2 * obj.ydim + error("Wrong params.Gram_clf_FL size.") + end + obj.Gram_clf_FL = params.Gram_clf_FL; + obj.params = rmfield(obj.params, 'Gram_clf_FL'); + else + if size(params.clf.P, 1) ~= 2*obj.ydim || size(params.clf.P, 2) ~= 2 * obj.ydim + error("Wrong params.clf.P size.") + end + obj.Gram_clf_FL = params.clf.P; + obj.params.clf = rmfield(obj.params.clf, 'P'); + end + elseif clf_type == 2 + if ~isfield(params, 'Kp_FL') && isfield(params, 'Kd_FL') + error("Define variable in the params called 'Kp_FL', p gain for the CLF"); + end + if ~isfield(params, 'Kd_FL') && isfield(params, 'Kp_FL') + error("Define variable in the params called 'Kd_FL', d gain for the CLF"); + end + if ~isfield(params, 'Q_FL') + disp("Setting Q matrix for CLF to default identity matrix."); + Q = eye(2*obj.ydim); + else + if length(params.Q_FL) == 1 + Q = params.Q_FL * eye(2*obj.ydim); + elseif size(params.Q_FL, 1) ~= 2*obj.ydim + error("Wrong Q_FL size.") + else + Q = params.Q_FL; + end + end + fprintf("Setting the lyapunov function based on the feedback gains specified. \n"); + Kp = params.Kp_FL; + Kd = params.Kd_FL; + A = [zeros(obj.ydim), eye(obj.ydim); + -Kp*eye(obj.ydim), -Kd*eye(obj.ydim)]; + obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P + elseif clf_type == 3 + if ~isfield(params, 'Q_FL') + disp("Setting Q matrix for CLF to default identity matrix."); + Q = eye(2*obj.ydim); + else + if length(params.Q_FL) == 1 + Q = params.Q_FL * eye(2*obj.ydim); + elseif size(params.Q_FL, 1) ~= 2*obj.ydim || size(params.Q_FL, 2) ~= 2*obj.ydim + error("Wrong Q_FL size.") + else + Q = params.Q_FL; + end + end + if ~isfield(params, 'R_FL') + error("Define params.R_FL (weight for the input), to set up based on the LQR."); + else + if length(params.R_FL) == 1 + R = params.R_FL * eye(obj.udim); + elseif size(params.R_FL, 1) ~= obj.udim || size(params.R_FL, 2) ~= obj.udim + error("Wrong R_FL size.") + else + R = params.R_FL; + end + end + A = [zeros(obj.ydim), eye(obj.ydim); zeros(obj.ydim), zeros(obj.ydim)]; + B = [zeros(obj.ydim); eye(obj.ydim)]; + [~, obj.Gram_clf_FL] = lqr(A, B, Q, R); + end + + if isfield(params, 'clf') && isfield(params.clf, 'rate') + obj.clf_rate = params.clf.rate; + obj.params.clf = rmfield(obj.params.clf, 'rate'); + elseif isfield(params, 'clf_rate') + obj.clf_rate = params.clf_rate; + obj.params = rmfield(obj.params, 'clf_rate'); + else + obj.clf_rate = min(eig(Q))/max(eig(obj.Gram_clf_FL))/eps; + fprintf("Setting clf rate automatically to %.3f \n", obj.clf_rate); + end +end \ No newline at end of file diff --git a/lib/@CtrlAffineSysFL/wrap_controller.m b/lib/@CtrlAffineSysFL/wrap_controller.m new file mode 100644 index 0000000..ccfde8d --- /dev/null +++ b/lib/@CtrlAffineSysFL/wrap_controller.m @@ -0,0 +1,32 @@ +function [u, extra_t] = wrap_controller(obj, x, mu_ref, with_slack, verbose, controller) +% Wrapper Function for feedback linearizing system +% Note +% Created in the notion that every control system would have this wrapper +% to be compatible with rollout_controller function +% But this makes it impossible to extract other information that needs +% system to evolve. +% ex) dV_hat = V(t+1) - V(t) / dt +% Suggestion: +% Instead of explicitly stating ode_func, define a conceptual +% function like 'evolve_system' and this function might return +% extra_t that can parse additional information. This would keep the +% concept consistent and also can expand it by adding function like +% 'wrap_evolve_system'. +% Input +% x +% mu +% extra_t: slack, feas, Vs +% Output +% u : feedback-linearized u +% extra_t: slack, feas, mu, y, dy, V + +% TODO: going to support +[mu, extra_t] = controller(x, mu_ref, with_slack, verbose); +u = obj.ctrlFeedbackLinearize(x, mu); +[y, dy, ~, ~, ~] = obj.eval_y(x); + +extra_t.mu = mu; +extra_t.y = y; +extra_t.dy = dy; +% extra_t recording section +end diff --git a/lib/sim/get_ctrl_ref_from_ctrl_previous.m b/lib/sim/get_ctrl_ref_from_ctrl_previous.m new file mode 100644 index 0000000..203393c --- /dev/null +++ b/lib/sim/get_ctrl_ref_from_ctrl_previous.m @@ -0,0 +1,15 @@ +function ctrl_ref = get_ctrl_ref_from_ctrl_previous(~, ctrl_previous, ... + ratio_ctrl_diff) +%% ctrl_ref = get_ctrl_ref_from_ctrl_previous(x, ctrl_previous, ratio_ctrl_diff) +% ratio_ctrl_diff: ratio(0.0~1.0) in the min-norm objective for minimizing the +% difference between u(mu) and u_prev(mu_prev) (default 0.0) +% if 0.0 : minimizes the norm of plain u. +% if 1.0: minimize the norm of plain (u_k-u_{k-1}) + if nargin < 3 + ratio_ctrl_diff = 1; + end + if ratio_ctrl_diff > 1 || ratio_ctrl_diff < 0 + error("ratio_ctrl_diff should be a value between 0 and 1."); + end + ctrl_ref = ratio_ctrl_diff * ctrl_previous; +end \ No newline at end of file diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m new file mode 100644 index 0000000..5adbee3 --- /dev/null +++ b/lib/sim/rollout_controller.m @@ -0,0 +1,434 @@ +function [xs, us, ts, extraout] = rollout_controller( ... + x0, plant_sys, control_sys, controller, T, varargin) +%% [xs, us, ts, extras] = rollout_controller( ... +%% x0, plant_sys, control_sys, controller, T, varargin) +%% [xs, us, ts, extras] = rollout_controller( ... +%% x0, plant_sys, control_sys, controller, T, ... +%% 'field1_name', field1_value, 'field2_name', field2_value) +% example: +% [xs, us, ts, extraout] = rollout_controller( ... +% x0, t0, plant_sys, control_sys, @control_sys.ctrlClfQp, T); +% Vs = extraout.Vs; +% In this function, the controller always takes 'with_slack', 'verbose' as +% additional input arguments. Make sure your own controller supports these +% fields with varargin. If you want to pass other varying input arguments +% to the controller, follow the below sample code: +% controller_handle = @(x, varargin) your_custom_controller(x, ... +% 'field1_name', field1_value, 'field2_name', field2_value, varargin{:}); +% [xs, us, ts, extraout] = rollout_controller( ... +% x0, t0, plant_sys, control_sys, controller_handle, T); +%% simulates plant_sys with ctrlClfQp controller of the control_sys +%% for [t0, t0+T] starting at the initial state x0. +%% INPUTS +% x0: initial state +% plant_sys: ControlAffineSys instance for the plant simulation. +% control_sys: ControlAffineSys instance for the model and the controller. +% controller: function handle for controller. +% examples: ctrlClfQp, ctrlCbfQp, ctrlCbfClfQp +% T: simulation time (time horizon) +%% Supported fields for varargin +% SIMULATION OPTIONS: +% t0: initial t (default: 0.0) +% u0: initial control input (if you want to specify the first input.) +% dt: sampling time (default: 0.01) +% end_event_function: function handle that ends the simulation, for more +% information, please check out matlab ode Events. +% ode_func: your own choice of ode_func for simulation. (default: ode45) +% CONTROLLER OPTIONS: +% with_slack: if 1: relax constraints, 0: hard constraints. (default 1) +% u_ref: reference signal of u. It can be a constant vector or a function +% handle that takes x as input. +% t_tolerance: time tolerance for ending the simulation (default 1e-10) +% DEBUG: +% verbose_level: +% 0: print no log (default) +% 1: print log of the rollout +% 2: print also the log of the controller +% if 'verbose' is set to 1(0), it sets verbose_level = 1(0) +%% OUTPUTS +% xs: trace of state (size: (plant_sys.xdim, total_k)) +% us: trace of control input (size: (control_sys.udim, total_k)) +% Note that the last control input in the array is actually not used +% in the simulation. It is appended to match the size with xs and ts. +% ts: trace of time (length: total_k) +%% Extra outputs (extraout) +% Vs: trace of the CLF values (size: (control_sys.n_clf, total_k)) +% Bs: trace of the CLF values (size: (control_sys.n_cbf, total_k)) +% feas: trace of feasibility of the optimization-based controller (length: total_k) +% slacks: trace of slack variables (size: (n_slack, total_k)) +% Note that n_slack depends on the choice of the controller. +% comp_times: trace of computation time for each timestep (length: total_k) +% ys: trace of the output defined for feedback linearization (size: (control_sys.ydim, total_k)) +% mus: trace of virtula control input for feedback linearizationb-based +% controller (size: (control_sys.udim, total_k)) +% end_with_event: 1 if the simulation ended with the end_event, else 0. +settings = parse_function_args(varargin{:}); + +% Feedback-linearizer Determinant +if isa(control_sys, 'CtrlAffineSysFL') + is_control_sys_FL = true; +else + is_control_sys_FL = false; +end + +if ~isfield(settings, 't0') + t0 = 0; +else + t0 = settings.t0; +end + +% TODO: reverse calculate mu0 from u0 +if ~isfield(settings, 'u0') + u0 = []; +else + if length(settings.u0) ~= control_sys.udim + error("u0 dimension does not match with control_sys.udim."); + end + u0 = settings.u0; +end + +if ~isfield(settings, 'u_ref') + u_ref = []; +else + if ~isa(settings.u_ref, 'function_handle') && ... + (length(settings.u_ref) ~= control_sys.udim) + error("u_ref dimension does not match with control_sys.udim."); + end + u_ref = settings.u_ref; +end + +if ~isfield(settings, 'mu0') + mu0 = []; +else + if ~is_control_sys_FL + error("mu0 is supported only for control_sys of CtrlAffineSysFL"); + end + if length(settings.mu0) ~= control_sys.udim + error("mu0 dimension does not match with control_sys.udim."); + end + mu0 = settings.mu0; +end + +if ~isfield(settings, 'mu_ref') + mu_ref = []; +else + if ~is_control_sys_FL + error("mu_ref is supported only for control_sys of CtrlAffineSysFL"); + end + if length(settings.mu_ref) ~= control_sys.udim + error("mu_ref dimension does not match with control_sys.udim."); + end + mu_ref = settings.mu_ref; +end + +if ~isempty(u0) && ~isempty(mu0) + error("Only one of u0 and mu0 should be provided."); +end + +if isempty(u_ref) && isempty(mu_ref) + ref_type = 0; +elseif ~isempty(u_ref) && ~isempty(mu_ref) + error("Only one of u_ref and mu_ref should be provided."); +elseif ~isempty(u_ref) + ref_type = 1; % reference signal is u. +else + ref_type = 2; % reference signal is mu. +end + +if ~isfield(settings, 'dt') + dt = 0.01; +else + dt = settings.dt; +end + +if ~isfield(settings, 'with_slack') + with_slack = 1; +else + with_slack = settings.with_slack; +end + +if ~isfield(settings, 'verbose_level') + if isfield(settings, 'verbose') + verbose_level = settings.verbose; + else + verbose_level = 0; + end +else + verbose_level = settings.verbose_level; +end + +if ~isfield(settings, 'end_event_function') + end_event_function = []; +else + end_event_function = settings.end_event_function; +end + +if ~isfield(settings, 'ode_func') + ode_func = @ode45; +else + ode_func = settings.ode_func; +end + +if ~isfield(settings, 't_tolerance') + t_tolerance = 1e-10; +else + t_tolerance = settings.t_tolerance; +end + +% Dimension check +if plant_sys.udim ~= control_sys.udim + error("plant_sys.udim and control_sys.udim should match each other.") +end +udim = plant_sys.udim; +if length(x0) ~= plant_sys.xdim + error("x0 dimension does not match with plant_sys.xdim."); +end +if isrow(x0) + x0 = x0'; +end + +% Initialize traces. +xs = x0; +ts = t0; +us = []; +% traces for extras, they remain empty if not returned. +feas = []; +comp_times = []; +slacks = []; +Vs = []; +Bs = []; +% traces for extras, specific to feedback linearize-based controller. +ys = []; +dys = []; +mus = []; +% traces for other extras +extraout = struct; + +% Initialize state & time. +x = x0; +t = t0; +u_prev = zeros(control_sys.udim, 1); +mu_prev = zeros(control_sys.udim, 1); + +end_simulation = false; +MAX_TIME = 20; +%% Run simulation. +% _t indicates variables for the current loop. +tstart = tic; +while ~end_simulation + %% Determine control input. + tstart = tic; % DEBUG + + if t == t0 && ~isempty(u0) + u = u0; + % Run dummy controller to get extra_t + % TODO: this is a bad practice, fix this. + + [~, extra_t] = controller(x, 'verbose', 0); + + extra_t.feas = 1; + extra_t.comp_time = 0; + elseif t == t0 && ~isempty(mu0) + mu = mu0; + u = control_sys.ctrlFeedbackLinearize(x, mu); + % Run dummy controller to get extra_t + % TODO: this is a bad practice, fix this. + [~, extra_t] = controller(x, 'verbose', 0); + extra_t.feas = 1; + extra_t.comp_time = 0; + else + if ref_type == 0 + [u, extra_t] = controller(x, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); + + elseif ref_type == 1 + if isa(u_ref, 'function_handle') + u_ref_t = u_ref(x, u_prev); + else + u_ref_t = u_ref; + end + [u, extra_t] = controller(x, 'u_ref', u_ref_t, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); + elseif ref_type == 2 + if isa(mu_ref, 'function_handle') + mu_ref_t = mu_ref(x, mu_prev); + else + mu_ref_t = mu_ref; + end + [u, extra_t] = controller(x, 'mu_ref', mu_ref_t, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); + end + end + if verbose_level >= 1 + print_log(t, x, u, extra_t); + end + + tend = toc(tstart); + + us = [us, u]; + extraout = fetch_other_extras(extraout, extra_t); + feas = [feas, extra_t.feas]; + if ~isfield(extra_t, 'comp_time') + comp_times = [comp_times, 0]; + else + comp_times = [comp_times, extra_t.comp_time]; + end + if with_slack + slacks = [slacks, extra_t.slack]; + end + if isfield(extra_t, 'Vs') + Vs = [Vs, extra_t.Vs]; + end + if isfield(extra_t, 'Bs') + Bs = [Bs, extra_t.Bs]; + end + if isfield(extra_t, 'y') + ys = [ys, extra_t.y]; + end + if isfield(extra_t, 'dy') + dys = [dys, extra_t.dy]; + end + if isfield(extra_t, 'mu') + mus = [mus, extra_t.mu]; + mu_prev = extra_t.mu; + end + + %% Run simulation for one time step. + t_end_t = min(t + dt, t0+T); + if ~isempty(end_event_function) + ode_opt = odeset('Events', end_event_function); + [ts_t, xs_t, t_event] = ode_func( ... + @(t, x) plant_sys.dynamics(t, x, u), ... + [t, t_end_t], x, ode_opt); + end_simulation = abs(ts_t(end) - (t0 + T)) MAX_TIME + break + end + end_simulation = abs(ts_t(end) - (t0 + T))=2)); +elseif ref_type == 1 + if isa(u_ref, 'function_handle') + u_ref_t = u_ref(x, u_prev); + else + u_ref_t = u_ref; + end + [u, extra_t] = controller(x, 'u_ref', u_ref_t, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); +elseif ref_type == 2 + if isa(mu_ref, 'function_handle') + mu_ref_t = mu_ref(x, mu_prev); + else + mu_ref_t = mu_ref; + end + [u, extra_t] = controller(x, 'mu_ref', mu_ref_t, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); +end +if verbose_level >= 1 + print_log(t, x, u, extra_t); +end + + +us = [us, u]; +extraout = fetch_other_extras(extraout, extra_t); +feas = [feas, extra_t.feas]; +comp_times = [comp_times, extra_t.comp_time]; +if with_slack + slacks = [slacks, extra_t.slack]; +end +if isfield(extra_t, 'Vs') + Vs = [Vs, extra_t.Vs]; +end +if isfield(extra_t, 'Bs') + Bs = [Bs, extra_t.Bs]; +end +if isfield(extra_t, 'y') + ys = [ys, extra_t.y]; +end +if isfield(extra_t, 'dy') + dys = [dys, extra_t.dy]; +end +if isfield(extra_t, 'mu') + mus = [mus, extra_t.mu]; +end + +%% Make extraout +extraout.feas = feas; +extraout.comp_times = comp_times; +if ~isempty(slacks) + extraout.slacks = slacks; +end +if ~isempty(Vs) + extraout.Vs = Vs; +end +if ~isempty(Bs) + extraout.Bs = Bs; +end +if ~isempty(ys) + extraout.ys = ys; +end +if ~isempty(dys) + extraout.dys = dys; +end +if ~isempty(mus) + extraout.mus = mus; +end +if ~isempty(end_with_event) + extraout.end_with_event = end_with_event; +end +end % end of the main function. + + +function extras = fetch_other_extras(extras, extra_t) + if length(fieldnames(extras)) == 0 + extras_field_name = fieldnames(extra_t); + else + extras_field_name = fieldnames(extras); + end + + n_field = length(extras_field_name); + for i_field = 1:n_field + if ~strcmp(extras_field_name{i_field}, 'feas') && ... + ~strcmp(extras_field_name{i_field}, 'comp_time') && ... + ~strcmp(extras_field_name{i_field}, 'slack') && ... + ~strcmp(extras_field_name{i_field}, 'Vs') && ... + ~strcmp(extras_field_name{i_field}, 'Bs') && ... + ~strcmp(extras_field_name{i_field}, 'y') && ... + ~strcmp(extras_field_name{i_field}, 'mu') && ... + ~strcmp(extras_field_name{i_field}, 'dy') + to_attach = extra_t.(extras_field_name{i_field}); + if ~isfield(extras, extras_field_name{i_field}) + extras.(extras_field_name{i_field}) = {}; + end + extras.(extras_field_name{i_field}){end+1} = to_attach; + end + end +end + +function print_log(t, x, u, extra_t) + fprintf("t: %.3f, \t x: ", t); + fprintf("%.2g, ", x); + fprintf("\t u: "); + fprintf("%.2g, ", u); + fprintf("\t feas: %d", extra_t.feas); + fprintf("\t comp_time: %.2f", extra_t.comp_time); + % Add custom log here. + fprintf("\n"); +end diff --git a/lib/sim/rollout_controller_for_multiple_resets.m b/lib/sim/rollout_controller_for_multiple_resets.m new file mode 100644 index 0000000..944f828 --- /dev/null +++ b/lib/sim/rollout_controller_for_multiple_resets.m @@ -0,0 +1,170 @@ +function [xs, us, ts, extras] = rollout_controller_for_multiple_resets(... + x0, plant_sys, control_sys, controller, ... + reset_event_function, reset_map_function, n_reset, ... + varargin) +%% Rollout control for the hybrid system that uses reset_map functions +%% Input: +% Necessary +% reset_event_function: function handle that detect the reset. +% reset_map_function: function handle that defines the reset map. +% n_reset: number of reset to simulate. +% Default +% varargin: with_slack, mu0, verbose, event_options +%% Supported fields for varargin +% T_exit: exit simulation if one rollout exit this time before +% hitting the reset map. +% exclude_pre_reset: excludes pre reset xs, us, ts, etc from the +% traces. (default: 0) +% exit_function: if this exit condition is satisfied, terminate +% simulation without going further. +% DEBUG: +% verbose_level: +% 0: print no log (default) +% 1: print log of each steps +% 2: print also the log of the rollout +% 3: print also the log of the controller +% if 'verbose' is set to 1(0), it sets verbose_level = 1(0) +%% Output: +% xs: trajectory +% us: control inputs +% ts: time stamps + +settings = parse_function_args(varargin{:}); + +if ~isfield(settings, 't0') + t0 = 0; +else + t0 = settings.t0; +end + +if ~isfield(settings, 'T_exit') + fprintf("Warning: T_exit is set to default value of 1e4. It is strongly %s", ... + "recommended to manually set T_exit to prevent unnecessary rollout.\n"); + T_exit = 1e4; +else + T_exit = settings.T_exit; +end + +if ~isfield(settings, 'exclude_pre_reset') + exclude_pre_reset = 0; +else + exclude_pre_reset = settings.exclude_pre_reset; +end + +if ~isfield(settings, 'exit_function') + exit_function = []; +else + exit_function = settings.exit_function; +end + +if ~isfield(settings, 'verbose_level') + if isfield(settings, 'verbose') + verbose_level = settings.verbose; + else + verbose_level = 0; + end +else + verbose_level = settings.verbose_level; +end + +verbose_level_rollout = max(verbose_level-1, 0); + +x = x0; +% initialize traces. +xs = []; +ts = []; +us = []; +index_reset = []; +extras = []; + +%% Run Step Simulation +for k = 1:n_reset + step_summary.x_init = x0; + [xs_new, us_new, ts_new, extras_new] = rollout_controller( ... + x0, plant_sys, control_sys, controller, T_exit,... + varargin{:}, 'verbose_level', verbose_level_rollout, 't0', t0, ... + 'end_event_function', reset_event_function); + end_with_reset = extras_new.end_with_event; + extras_new = rmfield(extras_new, 'end_with_event'); + step_summary.x_terminal = xs_new(:, end); + step_summary.T = ts_new(end) - ts_new(1); + step_summary.end_with_event = end_with_reset; + if verbose_level >= 1 + print_step_summary(k, step_summary); + end + % Save to log. + %% TODO: this can be changed based on the exit_function. + [exit_flag, exit_valid_index] = exit_function(xs_new); + if exclude_pre_reset + margin = 1; + else + margin = 0; + end + + if exit_flag + % normally exited + index_last = exit_valid_index - margin; + else + index_last = length(ts_new) - margin; + end + + extras = fetch_extras(extras, extras_new, index_last); + ts = [ts, ts_new(1:index_last)]; + xs = [xs, xs_new(:, 1:index_last)]; + us = [us, us_new(:, 1:index_last)]; + + if ~end_with_reset || exit_flag + % One step rollout terminated without hitting the reset. Terminate + % the simulation here. + break; + else + index_reset = [index_reset; length(ts)]; + end + %% reset to next initial state. + x0 = reset_map_function(xs_new(:, end)'); + t0 = ts_new(end); +end +extras.index_reset = index_reset; +end % end of the main function. + +function extras = fetch_extras(extras, extras_new, index_last) + if isempty(extras) + extras_field_name = fieldnames(extras_new); + else + extras_field_name = fieldnames(extras); + end + + n_field = length(extras_field_name); + for i_field = 1:n_field + to_attach = extras_new.(extras_field_name{i_field}); + if iscell(to_attach) + to_attach = to_attach(1:index_last); + if isfield(extras, extras_field_name{i_field}) + extras.(extras_field_name{i_field})(end+1:end+index_last) = ... + to_attach; + else + extras.(extras_field_name{i_field}) = to_attach; + end + else + otherdims = repmat({':'},1,ndims(to_attach)-1); + to_attach = to_attach(otherdims{:}, 1:index_last); + if isfield(extras, extras_field_name{i_field}) + extras.(extras_field_name{i_field}) = ... + cat(ndims(to_attach), extras.(extras_field_name{i_field}), to_attach); + else + extras.(extras_field_name{i_field}) = to_attach; + end + end + + end +end + +function print_step_summary(i, log) + fprintf("------Summary of %d-th step------\n", i); + disp("Initial state"); + disp(log.x_init'); + disp("Terminal state"); + disp(log.x_terminal'); + fprintf("Time spent: %.3f \t end_with_event: %d \n", [log.T, log.end_with_event]); + fprintf("------Resetting to next step------\n\n"); +end diff --git a/utils/io/parse_function_args.m b/utils/io/parse_function_args.m new file mode 100644 index 0000000..b537634 --- /dev/null +++ b/utils/io/parse_function_args.m @@ -0,0 +1,7 @@ +function kwargs = parse_function_args(varargin) +%% kwargs = parse_function_args(varargin{:}); + kwargs = struct(); + for idx = 1:2:length(varargin) + kwargs.(varargin{idx}) = varargin{idx+1}; + end +end \ No newline at end of file diff --git a/utils/io/parse_obj_args.m b/utils/io/parse_obj_args.m new file mode 100644 index 0000000..75edf3b --- /dev/null +++ b/utils/io/parse_obj_args.m @@ -0,0 +1,6 @@ +function obj = parse_obj_args(obj,varargin) +%% obj = parse_obj_args(obj, varargin{:}); + for idx = 1:2:length(varargin) + obj.(varargin{idx}) = varargin{idx+1} ; + end +end \ No newline at end of file diff --git a/utils/plot_helper/callPlotSettings.m b/utils/plot_helper/callPlotSettings.m new file mode 100644 index 0000000..e0988d4 --- /dev/null +++ b/utils/plot_helper/callPlotSettings.m @@ -0,0 +1,7 @@ +function callPlotSettings(fig_sz, plot_pos) + set(gca, 'FontName', 'Times New Roman'); + set(gcf, 'PaperPositionMode', 'manual'); + set(gcf, 'PaperUnits', 'inches'); + set(gcf, 'PaperSize', fig_sz); + set(gcf, 'PaperPosition', plot_pos); +end \ No newline at end of file diff --git a/utils/plot_helper/subplot1.m b/utils/plot_helper/subplot1.m new file mode 100644 index 0000000..beeec52 --- /dev/null +++ b/utils/plot_helper/subplot1.m @@ -0,0 +1,228 @@ +function subplot1(M,N,varargin); +%------------------------------------------------------------------------- +% subplot1 function An mproved subplot function +% Input : - If more than one input argumenst are given, +% then the first parameter is the number of rows. +% If single input argument is given, then this is the +% subplot-number for which to set focus. +% This could a scalar or two element vector (I,J). +% - Number of columns. +% * variable number of parameters +% (in pairs: ...,Keywoard, Value,...) +% - 'Min' : X, Y lower position of lowest subplot, +% default is [0.10 0.10]. +% - 'Max' : X, Y largest position of highest subplot, +% default is [0.95 0.95]. +% - 'Gap' : X,Y gaps between subplots, +% default is [0.01 0.01]. +% - 'XTickL' : x ticks labels option, +% 'Margin' : plot only XTickLabels in the +% subplot of the lowest row (default). +% 'All' : plot XTickLabels in all subplots. +% 'None' : don't plot XTickLabels in subplots. +% - 'YTickL' : y ticks labels option, +% 'Margin' : plot only YTickLabels in the +% subplot of the lowest row (defailt). +% 'All' : plot YTickLabels in all subplots. +% 'None' : don't plot YTickLabels in subplots. +% - 'FontS' : axis font size, default is 10. +% 'XScale' : scale of x axis: +% 'linear', default. +% 'log' +% - 'YScale' : scale of y axis: +% 'linear', default. +% 'log' +% Example: subplot1(2,2,'Gap',[0.02 0.02]); +% subplot1(2,3,'Gap',[0.02 0.02],'XTickL','None','YTickL','All','FontS',16); +% See also : subplot1c.m +% Tested : Matlab 5.3 +% By : Eran O. Ofek June 2002 +% URL : http://wise-obs.tau.ac.il/~eran/matlab.html +%------------------------------------------------------------------------- +MinDef = [0.10 0.10]; +MaxDef = [0.95 0.95]; +GapDef = [0.01 0.01]; +XTickLDef = 'Margin'; +YTickLDef = 'Margin'; +FontSDef = 10; +XScaleDef = 'linear'; +YScaleDef = 'linear'; + +% set default parameters +Min = MinDef; +Max = MaxDef; +Gap = GapDef; +XTickL = XTickLDef; +YTickL = YTickLDef; +FontS = FontSDef; +XScale = XScaleDef; +YScale = YScaleDef; + + +MoveFoc = 0; +if (nargin==1), + %--- move focus to subplot # --- + MoveFoc = 1; +elseif (nargin==2), + % do nothing +elseif (nargin>2), + Narg = length(varargin); + if (0.5.*Narg==floor(0.5.*Narg)), + + for I=1:2:Narg-1, + switch varargin{I}, + case 'Min' + Min = varargin{I+1}; + case 'Max' + Max = varargin{I+1}; + case 'Gap' + Gap = varargin{I+1}; + case 'XTickL' + XTickL = varargin{I+1}; + case 'YTickL' + YTickL = varargin{I+1}; + case 'FontS' + FontS = varargin{I+1}; + case 'XScale' + XScale = varargin{I+1}; + case 'YScale' + YScale = varargin{I+1}; + otherwise + error('Unknown keyword'); + end + end + else + error('Optional arguments should given as keyword, value'); + end +else + error('Illegal number of input arguments'); +end + + + + + + + + +switch MoveFoc + case 1 + %--- move focus to subplot # --- + H = get(gcf,'Children'); + Ptot = length(H); + if (length(M)==1), + M = Ptot - M + 1; + elseif (length(M)==2), + %--- check for subplot size --- + Pos1 = get(H(1),'Position'); + Pos1x = Pos1(1); + for Icheck=2:1:Ptot, + PosN = get(H(Icheck),'Position'); + PosNx = PosN(1); + if (PosNx==Pos1x), + NumberOfCol = Icheck - 1; + break; + end + end + NumberOfRow = Ptot./NumberOfCol; + + Row = M(1); + Col = M(2); + + M = (Row-1).*NumberOfCol + Col; + M = Ptot - M + 1; + else + error('Unknown option, undefined subplot index'); + end + + set(gcf,'CurrentAxes',H(M)); + + case 0 + %--- open subplots --- + + Xmin = Min(1); + Ymin = Min(2); + Xmax = Max(1); + Ymax = Max(2); + Xgap = Gap(1); + Ygap = Gap(2); + + + Xsize = (Xmax - Xmin)./N; + Ysize = (Ymax - Ymin)./M; + + Xbox = Xsize - Xgap; + Ybox = Ysize - Ygap; + + + Ptot = M.*N; + + Hgcf = gcf; + clf; + figure(Hgcf); + for Pi=1:1:Ptot, + Row = ceil(Pi./N); + Col = Pi - (Row - 1)*N; + + Xstart = Xmin + Xsize.*(Col - 1); + Ystart = Ymax - Ysize.*Row; + +% subplot(M,N,Pi); +% hold on; + axes('position',[Xstart,Ystart,Xbox,Ybox]); + %set(gca,'position',[Xstart,Ystart,Xbox,Ybox]); + set(gca,'FontSize',FontS); + box on; + hold on; + + switch XTickL + case 'Margin' + if (Row~=M), + %--- erase XTickLabel --- + set(gca,'XTickLabel',[]); + end + case 'All' + % do nothing + case 'None' + set(gca,'XTickLabel',[]); + otherwise + error('Unknown XTickL option'); + end + + switch YTickL + case 'Margin' + if (Col~=1), + %--- erase YTickLabel --- + set(gca,'YTickLabel',[]); + end + case 'All' + % do nothing + case 'None' + set(gca,'YTickLabel',[]); + otherwise + error('Unknown XTickL option'); + end + + switch XScale + case 'linear' + set(gca,'XScale','linear'); + case 'log' + set(gca,'XScale','log'); + otherwise + error('Unknown XScale option'); + end + + switch YScale + case 'linear' + set(gca,'YScale','linear'); + case 'log' + set(gca,'YScale','log'); + otherwise + error('Unknown YScale option'); + end + + end + + otherwise + error('Unknown MoveFoc option'); +end