From 3de948a571e3c274a1ecdb21de60028be86dedaf Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Tue, 20 Jul 2021 16:07:50 +0900 Subject: [PATCH 01/49] test commit --- a.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 a.txt diff --git a/a.txt b/a.txt new file mode 100644 index 0000000..dd457c7 --- /dev/null +++ b/a.txt @@ -0,0 +1 @@ +wwwwww From ec83b43794325cfc930820be9100be6ab1038d44 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Thu, 29 Jul 2021 20:03:52 +0900 Subject: [PATCH 02/49] Branch open Signed-off-by: goldenhazard --- lib/@CtrlAffineSys/CtrlAffineSys.m | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 71c432a..5da13fc 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -51,6 +51,7 @@ %% dynsys = CtrlAffineSys(params, 'symbolic') %% if user-defined dynamics, cbf, clf are used: %% dynsys = CtrlAffineSys(params, 'built-in') + disp(setup_option); if nargin < 1 error("Warning: params argument is missing.") end From c6e28e1007549e40dd479a6e338bb95c7103ebf5 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sat, 31 Jul 2021 22:11:34 +0900 Subject: [PATCH 03/49] FL version support-no-symbolic --- demos/cart_pole/bo_loss_func.m | 18 ++ demos/cart_pole/clip_theta.m | 10 + demos/cart_pole/init_settings_cart_pole.m | 50 ++++ demos/cart_pole/main_cart_pole.m | 21 ++ demos/cart_pole/main_cart_pole_BO.m | 137 +++++++++++ demos/cart_pole/main_cart_pole_FL.m | 62 +++++ demos/cart_pole/vis/draw_cart_pole.m | 81 +++++++ demos/cart_pole/vis/draw_rollout.m | 9 + demos/dynsys/@AccBuiltIn/AccBuiltIn.m | 65 +++++ demos/dynsys/@AccSymbolic/AccSymbolic.m | 16 ++ demos/dynsys/@AccSymbolic/defineCbf.m | 10 + demos/dynsys/@AccSymbolic/defineClf.m | 6 + demos/dynsys/@AccSymbolic/defineSystem.m | 15 ++ demos/dynsys/@CartPole/CartPole.m | 17 ++ demos/dynsys/@CartPole/bezier.m | 25 ++ demos/dynsys/@CartPole/defineClf.m | 40 ++++ demos/dynsys/@CartPole/defineOutput.m | 23 ++ demos/dynsys/@CartPole/defineSystem.m | 23 ++ .../@DoubleIntegrator2D/DoubleIntegrator2D.m | 26 ++ .../dynsys/@DoubleIntegrator2D/defineSystem.m | 12 + demos/dynsys/@DubinsCar/DubinsCar.m | 12 + demos/dynsys/@DubinsCar/defineCbf.m | 13 + demos/dynsys/@DubinsCar/defineClf.m | 6 + .../@InvertedPendulum/InvertedPendulum.m | 5 + demos/dynsys/@InvertedPendulum/defineClf.m | 12 + demos/dynsys/@InvertedPendulum/defineSystem.m | 11 + demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 66 ++++++ demos/init_clf_simulation_rabbit.m | 19 ++ demos/run_cbf_clf_simulation_acc.m | 4 +- demos/run_clf_simulation_rabbit.m | 161 +++++++++++++ dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 66 ++++++ lib/@CtrlAffineSys/CtrlAffineSys.m | 1 - lib/@CtrlAffineSys/ctrlClfQp.m | 3 +- lib/@CtrlAffineSys/init_sys.m | 9 +- lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 223 ++++++++++++++++-- lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m | 6 +- lib/@CtrlAffineSysFL/initOutputDynamics.m | 131 +++++----- lib/@CtrlAffineSysFL/init_sys_FL.m | 65 +++++ utils/rollout_feedback_linearization.m | 92 ++++++++ 39 files changed, 1482 insertions(+), 89 deletions(-) create mode 100644 demos/cart_pole/bo_loss_func.m create mode 100644 demos/cart_pole/clip_theta.m create mode 100644 demos/cart_pole/init_settings_cart_pole.m create mode 100644 demos/cart_pole/main_cart_pole.m create mode 100644 demos/cart_pole/main_cart_pole_BO.m create mode 100644 demos/cart_pole/main_cart_pole_FL.m create mode 100644 demos/cart_pole/vis/draw_cart_pole.m create mode 100644 demos/cart_pole/vis/draw_rollout.m create mode 100644 demos/dynsys/@AccBuiltIn/AccBuiltIn.m create mode 100644 demos/dynsys/@AccSymbolic/AccSymbolic.m create mode 100644 demos/dynsys/@AccSymbolic/defineCbf.m create mode 100644 demos/dynsys/@AccSymbolic/defineClf.m create mode 100644 demos/dynsys/@AccSymbolic/defineSystem.m create mode 100644 demos/dynsys/@CartPole/CartPole.m create mode 100644 demos/dynsys/@CartPole/bezier.m create mode 100644 demos/dynsys/@CartPole/defineClf.m create mode 100644 demos/dynsys/@CartPole/defineOutput.m create mode 100644 demos/dynsys/@CartPole/defineSystem.m create mode 100644 demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m create mode 100644 demos/dynsys/@DoubleIntegrator2D/defineSystem.m create mode 100644 demos/dynsys/@DubinsCar/DubinsCar.m create mode 100644 demos/dynsys/@DubinsCar/defineCbf.m create mode 100644 demos/dynsys/@DubinsCar/defineClf.m create mode 100644 demos/dynsys/@InvertedPendulum/InvertedPendulum.m create mode 100644 demos/dynsys/@InvertedPendulum/defineClf.m create mode 100644 demos/dynsys/@InvertedPendulum/defineSystem.m create mode 100644 demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m create mode 100644 demos/init_clf_simulation_rabbit.m create mode 100644 demos/run_clf_simulation_rabbit.m create mode 100644 dynsys/@RabbitBuiltIn/RabbitBuiltIn.m create mode 100644 lib/@CtrlAffineSysFL/init_sys_FL.m create mode 100644 utils/rollout_feedback_linearization.m 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..d8efb3f --- /dev/null +++ b/demos/cart_pole/init_settings_cart_pole.m @@ -0,0 +1,50 @@ +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.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/dynsys/@AccBuiltIn/AccBuiltIn.m b/demos/dynsys/@AccBuiltIn/AccBuiltIn.m new file mode 100644 index 0000000..97e21b9 --- /dev/null +++ b/demos/dynsys/@AccBuiltIn/AccBuiltIn.m @@ -0,0 +1,65 @@ +%% Main Reference +% Aaron Ames et al. Control Barrier Function based Quadratic Programs +% with Application to Adaptive Cruise Control, CDC 2014, Table 1. + +classdef AccBuiltIn < CtrlAffineSys + methods + function obj = AccBuiltIn(params) + % Always using built-in option for setup. + obj = obj@CtrlAffineSys(params, 'built-in'); + end + function Fr = getFr(obj, x) + v = x(2, :); + Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v.^2; + end + function f_ = f(obj, x) + v = x(2, :); + Fr = getFr(obj, x); + f_ = [v; -Fr/obj.params.m; obj.params.v0-v]; + end + function g_ = g(obj, x) + g_ = [0; 1/obj.params.m; 0]; + end + function V = clf(obj, x) + v = x(2, :); + V = (v - obj.params.vd).^2; + end + function LfV = lf_clf(obj, x) + v = x(2, :); + Fr = getFr(obj, x); + LfV = - 2 * Fr .* (v - obj.params.vd) / obj.params.m; + end + function LgV = lg_clf(obj, x) + v = x(2, :); + LgV = 2 * (v - obj.params.vd) / obj.params.m; + end + function B = cbf(obj, x) + v = x(2); + z = x(3); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); + end + function LfB = lf_cbf(obj, x) + v = x(2); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + m = obj.params.m; + Fr = getFr(obj, x); + LfB = (T + (v - v0) / (cd * g)) .* Fr / m + (v0 - v); + end + function LgB = lg_cbf(obj, x) + v = x(2); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + m = obj.params.m; + LgB = -(T + (v - v0) / (cd * g)) / m; + end + end +end diff --git a/demos/dynsys/@AccSymbolic/AccSymbolic.m b/demos/dynsys/@AccSymbolic/AccSymbolic.m new file mode 100644 index 0000000..bce030b --- /dev/null +++ b/demos/dynsys/@AccSymbolic/AccSymbolic.m @@ -0,0 +1,16 @@ +%% Main Reference +% Aaron Ames et al. Control Barrier Function based Quadratic Programs +% with Application to Adaptive Cruise Control, CDC 2014, Table 1. + +classdef AccSymbolic < CtrlAffineSys + methods + function obj = AccSymbolic(params) + % Always using symbolic option for setup. + obj = obj@CtrlAffineSys(params, 'symbolic'); + end + function Fr = getFr(obj, x) + v = x(2); + Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v^2; + end + end +end diff --git a/demos/dynsys/@AccSymbolic/defineCbf.m b/demos/dynsys/@AccSymbolic/defineCbf.m new file mode 100644 index 0000000..a6edb99 --- /dev/null +++ b/demos/dynsys/@AccSymbolic/defineCbf.m @@ -0,0 +1,10 @@ +function cbf = defineCbf(obj, params, symbolic_state) + v = symbolic_state(2); + z = symbolic_state(3); + + v0 = params.v0; + T = params.T; + cd = params.cd; + + cbf = z - T * v - 0.5 * (v0-v)^2 / (cd * params.g); +end \ No newline at end of file diff --git a/demos/dynsys/@AccSymbolic/defineClf.m b/demos/dynsys/@AccSymbolic/defineClf.m new file mode 100644 index 0000000..b2feb92 --- /dev/null +++ b/demos/dynsys/@AccSymbolic/defineClf.m @@ -0,0 +1,6 @@ +function clf = defineClf(obj, params, symbolic_state) + v = symbolic_state(2); + vd = params.vd; % desired velocity. + + clf = (v - vd)^2; +end \ No newline at end of file diff --git a/demos/dynsys/@AccSymbolic/defineSystem.m b/demos/dynsys/@AccSymbolic/defineSystem.m new file mode 100644 index 0000000..e9c9ad4 --- /dev/null +++ b/demos/dynsys/@AccSymbolic/defineSystem.m @@ -0,0 +1,15 @@ +function [x, f, g] = defineSystem(~, params) + syms p v z % states + x = [p; v; z]; + + f0 = params.f0; + f1 = params.f1; + f2 = params.f2; + v0 = params.v0; + m = params.m; + Fr = f0 + f1 * v + f2 * v^2; + + % Dynamics + f = [v; -Fr/m; v0-v]; + g = [0; 1/m; 0]; +end \ No newline at end of file diff --git a/demos/dynsys/@CartPole/CartPole.m b/demos/dynsys/@CartPole/CartPole.m new file mode 100644 index 0000000..42ca739 --- /dev/null +++ b/demos/dynsys/@CartPole/CartPole.m @@ -0,0 +1,17 @@ +classdef CartPole < CtrlAffineSysFL + methods + function E = getEnergy(obj, s) + l = obj.params.l; % [m] length of pendulum + m = obj.params.m; % [kg] mass of pendulum + M = obj.params.M; % [kg] mass of cart + b = obj.params.b; % [N/m/s] coefficient of friction between cart and ground + g = obj.params.g; % [m/s^2] acceleration of gravity + + % kinetic energy + T = 0.5 * (M+m) * s(2)^2 + m * s(2) * s(4) * l * cos(s(3)) + 0.5 * m * l^2 * s(4)^2; + % potential energy + U = -m * g * l * cos(s(3)); + E = T + U; + end + end +end diff --git a/demos/dynsys/@CartPole/bezier.m b/demos/dynsys/@CartPole/bezier.m new file mode 100644 index 0000000..472006d --- /dev/null +++ b/demos/dynsys/@CartPole/bezier.m @@ -0,0 +1,25 @@ +function fcn = bezier(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 + +end + +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 +end + diff --git a/demos/dynsys/@CartPole/defineClf.m b/demos/dynsys/@CartPole/defineClf.m new file mode 100644 index 0000000..4a6ad3f --- /dev/null +++ b/demos/dynsys/@CartPole/defineClf.m @@ -0,0 +1,40 @@ +function clf = defineClf(obj, params, symbolic_state) + s = symbolic_state; + E = getEnergy(obj, s); + E_ref = params.m * params.g * params.l; + + l = obj.params.l; % [m] length of pendulum + m = obj.params.m; % [kg] mass of pendulum + M = obj.params.M; % [kg] mass of cart + b = obj.params.b; % [N/m/s] coefficient of friction between cart and ground + g = obj.params.g; % [m/s^2] acceleration of gravity + A = [0, 1, 0, 0; + 0, 0, -m*g/M, 0; + 0, 0, 0, 1; + 0, 0, (m+M)*g/M, 0]; + B = [0; 1/M; 0; -1/(M*l)]; + Q = eye(4); + Q(1, 1) = 0.1; + Q(2, 2) = 0.1; + Q(3, 3) = 100; + Q(4, 4) = 20; + R = 0.1; + [K, P] = lqr(A, B, Q, R); + disp(P) + disp(eig(A-B*K)) + s_n = s; + s_n(3) = sin(s(3)); + s_n(4) = -s(4); + V_neighbor = s_n' * P * s_n; + + % clf = (1-cos(s(3))) * V_neighbor; + % clf = V_neighbor; + % clf = (E - E_ref)^2; + % clf = (E - E_ref)^2 + 5 * V_neighbor; + clf = (E - E_ref)^2 + 30 * (1-cos(s(3))) * V_neighbor; + % clf = abs(E - E_ref) * V_neighbor; + + % clf = (E - E_ref)^2 + 20 * exp(-(E - E_ref)^2) * V_neighbor; + + % clf = V_neighbor; +end \ No newline at end of file diff --git a/demos/dynsys/@CartPole/defineOutput.m b/demos/dynsys/@CartPole/defineOutput.m new file mode 100644 index 0000000..a3ffca8 --- /dev/null +++ b/demos/dynsys/@CartPole/defineOutput.m @@ -0,0 +1,23 @@ +function [y, phase, y_max_exceed, y_min_exceed] = defineOutput(obj, params, symbolic_state) + x = symbolic_state(1); + theta = symbolic_state(3); +% y = params.l * sin(theta); + phase = x + 0.5 * theta; + c_normal = 2.37079632679490; % normalization constant. + phase = phase / c_normal; + + y_desired = obj.bezier(params.bezier, phase); + y_desired_max = obj.bezier(params.bezier, params.phase_max); + y_desired_min = obj.bezier(params.bezier, params.phase_min); +% M = length(params.bezier)-1; % Degree of the bezier curve +% y_desired = sym(0); +% for i = 0:M +% y_desired = y_desired + ... +% params.bezier(i+1) * nchoosek(M, i) * (1-phase_var)^(M-i) * phase_var^i; +% end + + y_actual = theta; + y = y_actual - y_desired; + y_max_exceed = y_actual - y_desired_max; + y_min_exceed = y_actual - y_desired_min; +end \ No newline at end of file diff --git a/demos/dynsys/@CartPole/defineSystem.m b/demos/dynsys/@CartPole/defineSystem.m new file mode 100644 index 0000000..a53dece --- /dev/null +++ b/demos/dynsys/@CartPole/defineSystem.m @@ -0,0 +1,23 @@ +function [s, f, g] = defineSystem(obj, params) + +l = params.l; % [m] length of pendulum +m = params.m; % [kg] mass of pendulum +M = params.M; % [kg] mass of cart +b = params.b; % [N/m/s] coefficient of friction between cart and ground +g = params.g; % [m/s^2] acceleration of gravity + +syms x dx theta dtheta +% Theta: upright is pi (-pi) +s = [x; dx; theta; dtheta]; + +f = [s(2); + ( 2*m*l*s(4)^2*sin(s(3)) + 3*m*g*sin(s(3))*cos(s(3)) ... + - 4*b*s(2) )/( 4*(M+m)-3*m*cos(s(3))^2 ); + s(4); + (-3*m*l*s(4)^2*sin(s(3))*cos(s(3)) - 6*(M+m)*g*sin(s(3)) ... + - 6*(-b*s(2))*cos(s(3)) )/( 4*l*(m+M)-3*m*l*cos(s(3))^2 )]; + +g = [0; + 4/( 4*(M+m)-3*m*cos(s(3))^2 ); + 0; + (- 6*cos(s(3)) )/( 4*l*(m+M)-3*m*l*cos(s(3))^2 )]; diff --git a/demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m b/demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m new file mode 100644 index 0000000..95f4e6d --- /dev/null +++ b/demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m @@ -0,0 +1,26 @@ +classdef DoubleIntegrator2D < CtrlAffineSys + methods + function clf = defineClf(obj, params, symbolic_state) + x = symbolic_state; + + A = zeros(4); + A(1, 2) = 1; + A(3, 4) = 1; + B = [0 0; 1 0; 0 0; 0 1]; + Q = eye(size(A)); + R = eye(size(B,2)); + [~,P] = lqr(A,B,Q,R); + e = x - [params.p_d(1); 0; params.p_d(2); 0]; + clf = e' * P * e; + end + + function cbf = defineCbf(obj, params, symbolic_state) + x = symbolic_state; + p_o = params.p_o; % position of the obstacle. + r_o = params.r_o; % radius of the obstacle. + distance = (x(1) - p_o(1))^2 + (x(3) - p_o(2))^2 - r_o^2; + derivDistance = 2*(x(1)-p_o(1))*x(2) + 2*(x(3)-p_o(2))*x(4); + cbf = derivDistance + params.cbf_gamma0 * distance; + end + end +end diff --git a/demos/dynsys/@DoubleIntegrator2D/defineSystem.m b/demos/dynsys/@DoubleIntegrator2D/defineSystem.m new file mode 100644 index 0000000..2269459 --- /dev/null +++ b/demos/dynsys/@DoubleIntegrator2D/defineSystem.m @@ -0,0 +1,12 @@ +function [x, f, g] = defineSystem(~, ~) + syms p_x v_x p_y v_y; + x = [p_x; v_x; p_y; v_y]; + + A = zeros(4); + A(1, 2) = 1; + A(3, 4) = 1; + B = [0 0; 1 0; 0 0; 0 1]; + + f = A * x; + g = B; +end \ No newline at end of file diff --git a/demos/dynsys/@DubinsCar/DubinsCar.m b/demos/dynsys/@DubinsCar/DubinsCar.m new file mode 100644 index 0000000..46aa6b8 --- /dev/null +++ b/demos/dynsys/@DubinsCar/DubinsCar.m @@ -0,0 +1,12 @@ +classdef DubinsCar < CtrlAffineSys + methods + function [x, f, g] = defineSystem(obj, params) + syms p_x p_y theta + x = [p_x; p_y; theta]; + f = [params.v * cos(theta); + params.v * sin(theta); + 0]; + g = [0; 0; 1]; + end + end +end \ No newline at end of file diff --git a/demos/dynsys/@DubinsCar/defineCbf.m b/demos/dynsys/@DubinsCar/defineCbf.m new file mode 100644 index 0000000..78c3342 --- /dev/null +++ b/demos/dynsys/@DubinsCar/defineCbf.m @@ -0,0 +1,13 @@ +function cbf = defineCbf(~, params, symbolic_state) + x = symbolic_state; + p_x = x(1); p_y = x(2); theta = x(3); + + v = params.v; + xo = params.xo; + yo = params.yo; + d = params.d; + + distance = (p_x - xo)^2 + (p_y - yo)^2 - d^2; + derivDistance = 2*(p_x-xo)*v*cos(theta) + 2*(p_y-yo)*sin(theta); + cbf = derivDistance + params.cbf_gamma0 * distance; +end \ No newline at end of file diff --git a/demos/dynsys/@DubinsCar/defineClf.m b/demos/dynsys/@DubinsCar/defineClf.m new file mode 100644 index 0000000..3244a93 --- /dev/null +++ b/demos/dynsys/@DubinsCar/defineClf.m @@ -0,0 +1,6 @@ +function clf = defineClf(~, params, symbolic_state) + p_x = symbolic_state(1); + p_y = symbolic_state(2); + theta = symbolic_state(3); + clf = (cos(theta).*(p_y-params.yd)-sin(theta).*(p_x-params.xd)).^2; +end \ No newline at end of file diff --git a/demos/dynsys/@InvertedPendulum/InvertedPendulum.m b/demos/dynsys/@InvertedPendulum/InvertedPendulum.m new file mode 100644 index 0000000..286d96a --- /dev/null +++ b/demos/dynsys/@InvertedPendulum/InvertedPendulum.m @@ -0,0 +1,5 @@ +classdef InvertedPendulum < CtrlAffineSys + methods + + end +end diff --git a/demos/dynsys/@InvertedPendulum/defineClf.m b/demos/dynsys/@InvertedPendulum/defineClf.m new file mode 100644 index 0000000..cc3a122 --- /dev/null +++ b/demos/dynsys/@InvertedPendulum/defineClf.m @@ -0,0 +1,12 @@ +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; +end + diff --git a/demos/dynsys/@InvertedPendulum/defineSystem.m b/demos/dynsys/@InvertedPendulum/defineSystem.m new file mode 100644 index 0000000..bbb1359 --- /dev/null +++ b/demos/dynsys/@InvertedPendulum/defineSystem.m @@ -0,0 +1,11 @@ +function [x, f, g] = defineSystem(obj, params) + syms theta dtheta + x = [theta; dtheta]; + l = params.l; % [m] length of pendulum + m = params.m; % [kg] mass of pendulum + g = params.g; % [m/s^2] acceleration of gravity + b = params.b; % [s*Nm/rad] friction coefficient + + f = [x(2); (-b*x(2) + m*g*l*sin(x(1))/2 ) / (m*l^2/3)]; + g = [0; -1 / (m*l^2/3)]; +end \ No newline at end of file diff --git a/demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m new file mode 100644 index 0000000..3638e73 --- /dev/null +++ b/demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -0,0 +1,66 @@ +%% Main Reference +% Aaron Ames et al. Control Barrier Function based Quadratic Programs +% with Application to Adaptive Cruise Control, CDC 2014, Table 1. + +classdef RabbitBuiltIn < CtrlAffineSys + methods + function obj = RabbitBuiltIn(params) + % Always using built-in option for setup. + obj = obj@CtrlAffineSys(params, 'built-in'); + end + function Fr = getFr(obj, x) + v = x(2, :); + Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v.^2; + end + function f_ = f(obj, x) + v = x(2, :); + Fr = getFr(obj, x); + f_ = [v; -Fr/obj.params.m; obj.params.v0-v]; + end + function g_ = g(obj, x) + g_ = [0; 1/obj.params.m; 0]; + end + function V = clf(obj, x) + v = x(2, :); + V = (v - obj.params.vd).^2; + end + function LfV = lf_clf(obj, x) + v = x(2, :); + Fr = getFr(obj, x); + LfV = - 2 * Fr .* (v - obj.params.vd) / obj.params.m; + end + function LgV = lg_clf(obj, x) + v = x(2, :); + LgV = 2 * (v - obj.params.vd) / obj.params.m; + end + function B = cbf(obj, x) + v = x(2); + z = x(3); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); + end + function LfB = lf_cbf(obj, x) + v = x(2); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + m = obj.params.m; + Fr = getFr(obj, x); + LfB = (T + (v - v0) / (cd * g)) .* Fr / m + (v0 - v); + end + function LgB = lg_cbf(obj, x) + v = x(2); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + m = obj.params.m; + LgB = -(T + (v - v0) / (cd * g)) / m; + end + end +end + diff --git a/demos/init_clf_simulation_rabbit.m b/demos/init_clf_simulation_rabbit.m new file mode 100644 index 0000000..d093131 --- /dev/null +++ b/demos/init_clf_simulation_rabbit.m @@ -0,0 +1,19 @@ +%% Initialize Global Simulation Configuration of Rabbit +global scale +global a_bez theta_end theta_init +global mu +global uSat +global apply_rl_adaptation +global control_type +global motor_dynamics + +motor_dynamics = 1; +apply_rl_adaptation = 0; +control_type = 1; % 0: Original(Westervelt) 1: CLF-QP 2: L1-CLF-QP 3: RL-CLF-QP +% IO-RL+CLF: apply_rl_adaptation=0, control_type=1 +uSat = 150; +mu = 0.8; % friction constraint; +theta_end = 3.384545423940834; +theta_init = 2.903323878938605; +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]; +scale = 2; \ No newline at end of file diff --git a/demos/run_cbf_clf_simulation_acc.m b/demos/run_cbf_clf_simulation_acc.m index b67b98e..250202f 100644 --- a/demos/run_cbf_clf_simulation_acc.m +++ b/demos/run_cbf_clf_simulation_acc.m @@ -34,8 +34,8 @@ params.udim = 1; %% Either option works. -accSys = AccSymbolic(params); -% accSys = AccBuiltIn(params); +% accSys = AccSymbolic(params); +accSys = AccBuiltIn(params); odeFun = @accSys.dynamics; controller = @accSys.ctrlCbfClfQp; diff --git a/demos/run_clf_simulation_rabbit.m b/demos/run_clf_simulation_rabbit.m new file mode 100644 index 0000000..ae0596b --- /dev/null +++ b/demos/run_clf_simulation_rabbit.m @@ -0,0 +1,161 @@ +%% RABBIT SIMULATION +close all; +clear all; + +%% Set simulation config: global variable +init_clf_simulation_rabbit; % initialize global variables +dt = 0.02; % simulation interval +sim_t = 30; % simulation time +nstep = 5; % number of steps + +%% Initialize state +% Init state. +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]; +x0 = [q0_ini;dq0_ini]; +% Rellabelling matrix +R = [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]; +%% Fullfill params +params.xdim = length(x0); +params.udim = 4; % TODO hard-coding +% Target position +params.p_d = [10; 0]; +% obstacle center +params.p_o = [5; 2]; +% obstacle radius. +params.r_o = 2; + +params.cbf_gamma0 = 1; + +params.u_max = 4; +params.u_min = -4; + +params.clf.rate = 0.7; +params.cbf.rate = 3; + +params.weight.slack = 1; +params.weight.input = 5; + + + +%% Prepare System +dynsys = RabbitBuiltIn(params); +odeFun = @dynsys.dynamics; +controller = @dynsys.ctrlClfQp; +odeSolver = @ode45; + +%% Prepare record paper +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); +Fsts = zeros(total_k-1, 2); + +xs(1, :) = x0'; +ts(1) = t; +u_prev = [0;0]; % Todo + +if motor_dynamics + s0_short2 = s0; + s0_motors = zeros(4, 1); + s0 = [s0_short2; s0_motors]; +end + +%% Main Simulation +for k = 1:total_k-1 + t + % Determine control input. + % dV_hat: analytic Vdot based on model. + [u, slack, V] = controller(x); +% [u, slack, h, V] = controller(s, u_prev); % optimizing the difference between the previous timestep. + 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); + u_prev = u; + t = t + dt; +end + +plot_results(ts, xs, us, params.p_o, params.r_o) + +%% Auxilary Functions +function plot_results(t, xs, us, p_o, r_o) + +figure +subplot(5,1,1) +plot(t, xs(:,1)) +xlabel('t') +ylabel('x [m]') + +subplot(5,1,2) +plot(t, xs(:,2)) +xlabel('t') +ylabel('v_x [m/s]') + +subplot(5,1,3) +plot(t, xs(:,3)) +xlabel('t') +ylabel('y [m]') + +subplot(5,1,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)) +xlabel('t') +ylabel('v [m/s]') + + +figure +subplot(2,1,1) +plot(t(1:end-1), us(:,1)) +xlabel('t') +ylabel('a_x [m/s^2]') + +subplot(2,1,2) +plot(t(1:end-1), 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([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)); +draw_circle(p_o, r_o); + +xlim([lim_min, lim_max]); +ylim([lim_min, lim_max]); +xlabel('x [m]') +ylabel('y [m]') +end + +function h = draw_circle(center,r) +hold on +th = 0:pi/50:2*pi; +xunit = r * cos(th) + center(1); +yunit = r * sin(th) + center(2); +h = plot(xunit, yunit); +hold off +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..eb5f14a --- /dev/null +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -0,0 +1,66 @@ +%% 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 + methods + function obj = RabbitBuiltIn(params) + % Always using built-in option for setup. + obj = obj@CtrlAffineSysFL(params, 'built-in'); + end + function Fr = getFr(obj, x) + v = x(2, :); + Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v.^2; + end + function f_ = f(obj, x) + v = x(2, :); + Fr = getFr(obj, x); + f_ = [v; -Fr/obj.params.m; obj.params.v0-v]; + end + function g_ = g(obj, x) + g_ = [0; 1/obj.params.m; 0]; + end + function V = clf(obj, x) + v = x(2, :); + V = (v - obj.params.vd).^2; + end + function LfV = lf_clf(obj, x) + v = x(2, :); + Fr = getFr(obj, x); + LfV = - 2 * Fr .* (v - obj.params.vd) / obj.params.m; + end + function LgV = lg_clf(obj, x) + v = x(2, :); + LgV = 2 * (v - obj.params.vd) / obj.params.m; + end + function B = cbf(obj, x) + v = x(2); + z = x(3); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); + end + function LfB = lf_cbf(obj, x) + v = x(2); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + m = obj.params.m; + Fr = getFr(obj, x); + LfB = (T + (v - v0) / (cd * g)) .* Fr / m + (v0 - v); + end + function LgB = lg_cbf(obj, x) + v = x(2); + T = obj.params.T; + cd = obj.params.cd; + v0 = obj.params.v0; + g = obj.params.g; + m = obj.params.m; + LgB = -(T + (v - v0) / (cd * g)) / m; + end + end +end + diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 5da13fc..71c432a 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -51,7 +51,6 @@ %% dynsys = CtrlAffineSys(params, 'symbolic') %% if user-defined dynamics, cbf, clf are used: %% dynsys = CtrlAffineSys(params, 'built-in') - disp(setup_option); if nargin < 1 error("Warning: params argument is missing.") end diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index 7c544aa..8737d8d 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -48,10 +48,11 @@ % Add input constraints if u_max or u_min exists. if isfield(obj.params, 'u_max') A = [A; eye(obj.udim), zeros(obj.udim, 1);]; - if size(obj.params.u_max, 1) == 1 + if size(obj.params.u_max, 1) == 1 % u_max is in column_vector form b = [b; obj.params.u_max * ones(obj.udim, 1)]; elseif size(obj.params.u_max, 1) == obj.udim b = [b; obj.params.u_max]; + % Get no error when u_max = (u_dim, x != 1) => TODO else error("params.u_max should be either a scalar value or an (udim, 1) array.") end diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index fc24de4..745f004 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -1,4 +1,8 @@ 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') @@ -56,6 +60,8 @@ 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 @@ -84,6 +90,7 @@ 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; @@ -118,7 +125,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.umax % what? error("Invalid size of params.u_max."); else if isrow(params.u_max) diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index 9e5963e..3100aee 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -11,37 +11,50 @@ % 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 % 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 + 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 + + obj@CtrlAffineSys(params, setup_option); + obj.init_sys_FL(params); - [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); + %[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); end function [y, phase, y_max_exceed, y_min_exceed] = defineOutput(obj, params, symbolic_state) @@ -95,5 +108,173 @@ LgLfy = obj.lglf_y(s); end end + + %% Sym2Value Function + % Help convenient use of function handlers regardless of built-in + % and symbolic + function y_ = y(obj, x) + % lg_cbf_ = lg_cbf(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(x) should be overriden by user."); + end + y_ = obj.y_sym(x); + end + + function phase_ = phase(obj, x) + % lg_cbf_ = lg_cbf(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.phase(x) should be overriden by user."); + end + disp(nargin); + if nargin < 2 + phase_ = []; + return + end + phase_ = obj.phase_sym(x); + end + + function lf_y_ = lf_y(obj, x) + % lg_cbf_ = lg_cbf(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(x) should be overriden by user."); + end + lf_y_ = obj.lf_y_sym(x); + end + + function lg_y_ = lg_y(obj, x) + % lg_cbf_ = lg_cbf(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(x) should be overriden by user."); + end + lg_y_ = obj.lg_y_sym(x); + end + + function lglf_y_ = lglf_y(obj, x) + % lg_cbf_ = lg_cbf(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(x) should be overriden by user."); + end + lglf_y_ = obj.lglf_y_sym(x); + end + + function l2f_y_ = l2f_y(obj, x) + % lg_cbf_ = lg_cbf(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(x) should be overriden by user."); + end + l2f_y_ = obj.l2f_y_sym(x); + end + + function y_max_exceed_ = y_max_exceed(obj, x) + % lg_cbf_ = lg_cbf(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_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) + % lg_cbf_ = lg_cbf(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_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_cbf_ = lg_cbf(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_max_exceed(x) should be overriden by user."); + end + lg_y_max_exceed_ = obj.lglf_y_max_exceed_sym(x); + end + + function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) + % lg_cbf_ = lg_cbf(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) + % lg_cbf_ = lg_cbf(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) + % lg_cbf_ = lg_cbf(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_ = y_min_exceed_sym(x); + end + + function lf_y_min_exceed_ = lf_y_min_exceed(x) + % lg_cbf_ = lg_cbf(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_ = lf_y_min_exceed_sym(x); + end + + function lg_y_min_exceed_ = lg_y_min_exceed(x) + % lg_cbf_ = lg_cbf(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_ = lg_y_min_exceed_sym(x); + end + + function lglf_y_min_exceed_ = lglf_y_min_exceed(x) + % lg_cbf_ = lg_cbf(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_ = lglf_y_min_exceed_sym(x); + end + + function l2f_y_min_exceed_ = l2f_y_min_exceed(x) + % lg_cbf_ = lg_cbf(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_ = l2f_y_min_exceed_sym(x); + end end end diff --git a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m index 383c519..e4bf1a6 100644 --- a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m +++ b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m @@ -24,15 +24,17 @@ end u = u_raw; + +%% Clip input into the physical constraint if isfield(obj.params, 'u_max') - if size(obj.params.u_max, 1) == 1 + if size(obj.params.u_max, 1) == 1 % row vector for i = 1:obj.udim if u(i) > obj.params.u_max u(i) = obj.params.u_max; disp("Warning: virtual control input does not satisfy input constraint.") end end - elseif size(obj.params.u_max, 1) == obj.udim + elseif size(obj.params.u_max, 1) == obj.udim % column vector for i = 1:obj.udim if u(i) > obj.params.u_max(i) u(i) = obj.params.u_max(i); 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..a9164df --- /dev/null +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -0,0 +1,65 @@ +function init_sys_FL(obj, params) + %% Functions that initialize dynamic system + obj.params = params; + 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); + + 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? + obj.ydim = obj.udim; + + else + error("Undefined setup_option."); + 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 +end + diff --git a/utils/rollout_feedback_linearization.m b/utils/rollout_feedback_linearization.m new file mode 100644 index 0000000..89fee2e --- /dev/null +++ b/utils/rollout_feedback_linearization.m @@ -0,0 +1,92 @@ +function [xs, us, ts, Vs, dVs_error, ys] = rollout_feedback_linearization( ... + x0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose) +if nargin < 7 + with_slack = 0; +elseif isempty(with_slack) + with_slack = 0; +end + +if nargin < 8 + mu0 = []; +end + +if nargin < 9 + verbose = 0; % run queitly +end + +% Total time step +total_k = ceil(sim_t / dt); + +% Initialize traces. +xs = zeros(total_k, plant_sys.xdim); % trace of the simulation. +ys = zeros(total_k, 1); +us = zeros(total_k - 1, control_sys.udim); +ts = zeros(total_k, 1); +Vs = zeros(total_k, 1); +dVs_error = zeros(total_k - 1, 1); + +x = x0; xs(1, :) = x0; +t = 0; ts(1) = t; + +[y, dy, ~, ~, phase] = control_sys.eval_y(x) +ys(1) = y; +Vs(1) = control_sys.clf_FL(y, dy); +%% Run Simulation +for k = 1:total_k-1 + % Evaluate CLf related values + V = control_sys.clf_FL(y, dy); + LfV = control_sys.lF_clf_FL(y, dy); + LgV = control_sys.lG_clf_FL(y, dy); + + %% Determine control input + if k ==1 && ~isempty(mu0) + % Apply predetermined control input for the initial step. + mu = mu; + u = control_sys.ctrlFeedbackLinearize(x, mu); + dV_hat = LfV + LgV * mu; % model based estimate of dV + else + % Determin control input from the controller. + mu = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); + u = control_sys.ctrlFeedbackLinearize(x, mu); + %% for debug; +% u = 0; +% feedforward = -control_sys.lglf_y(x)\control_sys.l2f_y(x); +% mu = control_sys.lglf_y(x) * (u - feedforward); + dV_hat = LfV + LgV * mu; % model based estimate of dV + end + us(k, :) = u; + + % Run simulation for one time step with determined control input. + [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); + t = ts_k(end); + ts(k+1) = t; + x_next = xs_k(end, :)'; + % x_next = clip_theta(xs_k(end, :))'; + xs(k+1, :) = x_next; + + % Evaluate CLf related values after one time step. + [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); + ys(k+1, 1) = y_next; + V_next = control_sys.clf_FL(y_next, dy_next); + LfV_next = control_sys.lF_clf_FL(y_next, dy_next); + LgV_next = control_sys.lG_clf_FL(y_next, dy_next); + % Estimating Vdot based on second order numerical differentiation. + dV_hat_next = LfV_next + LgV_next * mu; + dV_hat = (dV_hat + dV_hat_next)/2; + % Estimation of actual Vdot based on numerical differentiation + dV = (V_next - V) / dt; + dVs_error(k) = dV - dV_hat; + Vs(k+1) = V_next; +% if abs(dV-dV_hat) > 50 +% disp("debug"); +% end + if verbose + fprintf("mu: %.4f \t dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t LgV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... + [mu, dV, dV_hat, dV-dV_hat, LfV, LgV, x(3), x(4)]); + end + + x = x_next; + y = y_next; + dy = dy_next; + phase = phase_next; +end % end of main for loop \ No newline at end of file From 895fccfdb792d9e5f45306953ae1dbe32282106a Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sat, 31 Jul 2021 22:50:28 +0900 Subject: [PATCH 04/49] temp --- lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index 3100aee..907e18f 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -129,11 +129,11 @@ if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.phase(x) should be overriden by user."); end - disp(nargin); - if nargin < 2 - phase_ = []; - return - end +% disp(nargin); +% if nargin < 2 +% phase_ = []; +% return +% end phase_ = obj.phase_sym(x); end @@ -204,7 +204,7 @@ 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.lglf_y_max_exceed_sym(x); + lg_y_max_exceed_ = obj.lg_y_max_exceed_sym(x); end function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) @@ -234,7 +234,7 @@ 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_ = y_min_exceed_sym(x); + y_min_exceed_ = obj.y_min_exceed_sym(x); end function lf_y_min_exceed_ = lf_y_min_exceed(x) @@ -244,7 +244,7 @@ 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_ = lf_y_min_exceed_sym(x); + lf_y_min_exceed_ = obj.lf_y_min_exceed_sym(x); end function lg_y_min_exceed_ = lg_y_min_exceed(x) @@ -254,7 +254,7 @@ 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_ = lg_y_min_exceed_sym(x); + lg_y_min_exceed_ = obj.lg_y_min_exceed_sym(x); end function lglf_y_min_exceed_ = lglf_y_min_exceed(x) @@ -264,7 +264,7 @@ 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_ = lglf_y_min_exceed_sym(x); + lglf_y_min_exceed_ = obj.lglf_y_min_exceed_sym(x); end function l2f_y_min_exceed_ = l2f_y_min_exceed(x) @@ -274,7 +274,7 @@ 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_ = l2f_y_min_exceed_sym(x); + l2f_y_min_exceed_ = obj.l2f_y_min_exceed_sym(x); end end end From 7d9263c876c328a42337536504b9bed3b9661062 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sat, 31 Jul 2021 23:06:22 +0900 Subject: [PATCH 05/49] Trouble-shooting completed Signed-off-by: goldenhazard --- lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 33 +++++++++++++------------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index 907e18f..acb04b3 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -82,14 +82,14 @@ end function [y, dy, L2fy, LgLfy, phase] = eval_y(obj, s) - if isempty(obj.phase) - y = obj.y(s); - dy = obj.lf_y(s); - L2fy = obj.l2f_y(s); - LgLfy = obj.lglf_y(s); - phase = []; - return - end +% if isempty(obj.phase) +% y = obj.y(s); +% dy = obj.lf_y(s); +% L2fy = obj.l2f_y(s); +% LgLfy = obj.lglf_y(s); +% phase = []; +% return +% end phase = obj.phase(s); if phase > obj.params.phase_max y = obj.y_max_exceed(s); @@ -107,6 +107,10 @@ L2fy = obj.l2f_y(s); LgLfy = obj.lglf_y(s); end + y + dy + L2fy + LgLfy end %% Sym2Value Function @@ -129,11 +133,6 @@ if strcmp(obj.setup_option, 'built-in') error("For 'built-in' setup_option, obj.phase(x) should be overriden by user."); end -% disp(nargin); -% if nargin < 2 -% phase_ = []; -% return -% end phase_ = obj.phase_sym(x); end @@ -237,7 +236,7 @@ y_min_exceed_ = obj.y_min_exceed_sym(x); end - function lf_y_min_exceed_ = lf_y_min_exceed(x) + function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) % lg_cbf_ = lg_cbf(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)}. @@ -247,7 +246,7 @@ lf_y_min_exceed_ = obj.lf_y_min_exceed_sym(x); end - function lg_y_min_exceed_ = lg_y_min_exceed(x) + function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) % lg_cbf_ = lg_cbf(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)}. @@ -257,7 +256,7 @@ lg_y_min_exceed_ = obj.lg_y_min_exceed_sym(x); end - function lglf_y_min_exceed_ = lglf_y_min_exceed(x) + function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) % lg_cbf_ = lg_cbf(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)}. @@ -267,7 +266,7 @@ lglf_y_min_exceed_ = obj.lglf_y_min_exceed_sym(x); end - function l2f_y_min_exceed_ = l2f_y_min_exceed(x) + function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) % lg_cbf_ = lg_cbf(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)}. From 0a8003833045de8f0bf91f04b6df2f76f88c2430 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sat, 31 Jul 2021 23:39:23 +0900 Subject: [PATCH 06/49] nonsymbolic support version - FL added --- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 54 +++++++++++++++++++++++ lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 61 ++++++++++++++------------ lib/@CtrlAffineSysFL/init_sys_FL.m | 6 ++- 3 files changed, 90 insertions(+), 31 deletions(-) diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index eb5f14a..7fe374f 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -61,6 +61,60 @@ m = obj.params.m; LgB = -(T + (v - v0) / (cd * g)) / m; end + + function y_ = y(obj, x) + % TODO: Rabbit's output + end + + function phase_ = phase(obj, x) + % TODO: Rabbit's phase + end + + function Lfy = lf_y(obj, x) + % TODO: Rabbit's lie derivate of Lfy + end + + function Lgy = lg_y(obj, x) + % TODO: Rabbit's lie derivative of Lgy + end + + function LgLfy = lglf_y(obj, x) + % TODO: Rabbit's lie derivative of LgLfy + end + + function L2fy = l2f_y(obj, x) + % TODO: Rabbit's lie derivative of L2fy + end + + function y_max_exceed_ = y_max_exceed(obj, x) + end + + function lf_y_max_exceed_ = lf_y_max_exceed(obj, x) + end + + function lg_y_max_exceed_ = lg_y_max_exceed(obj, x) + end + + function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) + end + + function l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) + end + + function y_min_exceed_ = y_min_exceed(obj, x) + end + + function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) + end + + function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) + end + + function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) + end + + function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) + end end end diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index acb04b3..25371cb 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -82,6 +82,7 @@ end function [y, dy, L2fy, LgLfy, phase] = eval_y(obj, s) + % Todo: interpret this and make it more robust! % if isempty(obj.phase) % y = obj.y(s); % dy = obj.lf_y(s); @@ -107,19 +108,17 @@ L2fy = obj.l2f_y(s); LgLfy = obj.lglf_y(s); end - y - dy - L2fy - LgLfy 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) - % lg_cbf_ = lg_cbf(obj, x) + % y_ = y(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)}. + % 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 @@ -127,9 +126,9 @@ end function phase_ = phase(obj, x) - % lg_cbf_ = lg_cbf(obj, x) + % phase_ = phase(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)}. + % 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 @@ -137,9 +136,10 @@ end function lf_y_ = lf_y(obj, x) - % lg_cbf_ = lg_cbf(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 CBF L_g{B(x)}. + % 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 @@ -147,9 +147,9 @@ end function lg_y_ = lg_y(obj, x) - % lg_cbf_ = lg_cbf(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 CBF L_g{B(x)}. + % 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 @@ -157,9 +157,9 @@ end function lglf_y_ = lglf_y(obj, x) - % lg_cbf_ = lg_cbf(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 CBF L_g{B(x)}. + % 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 @@ -167,9 +167,10 @@ end function l2f_y_ = l2f_y(obj, x) - % lg_cbf_ = lg_cbf(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 CBF L_g{B(x)}. + % 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 @@ -177,9 +178,9 @@ end function y_max_exceed_ = y_max_exceed(obj, x) - % lg_cbf_ = lg_cbf(obj, x) + % y_max_exceed_ = 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)}. + % 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 @@ -187,9 +188,10 @@ end function lf_y_max_exceed_ = lf_y_max_exceed(obj, x) - % lg_cbf_ = lg_cbf(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 CBF L_g{B(x)}. + % 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 @@ -197,9 +199,10 @@ end function lg_y_max_exceed_ = lg_y_max_exceed(obj, x) - % lg_cbf_ = lg_cbf(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 CBF L_g{B(x)}. + % 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 @@ -207,7 +210,7 @@ end function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') @@ -217,7 +220,7 @@ end function l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') @@ -227,7 +230,7 @@ end function y_min_exceed_ = y_min_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') @@ -237,7 +240,7 @@ end function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') @@ -247,7 +250,7 @@ end function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') @@ -257,7 +260,7 @@ end function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') @@ -267,7 +270,7 @@ end function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) - % lg_cbf_ = lg_cbf(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') diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index a9164df..2d6b4bc 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -1,6 +1,7 @@ function init_sys_FL(obj, params) %% Functions that initialize dynamic system obj.params = params; + %% Symbolic Function if strcmp(obj.setup_option, 'symbolic') disp(['Setting up feedback linearization dynamics, CLFs, CBFs from defined symbolic expressions.', ... '(This might take time.)']); @@ -8,17 +9,18 @@ function init_sys_FL(obj, 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? obj.ydim = obj.udim; - else error("Undefined setup_option."); end - %% Set up desired linear output dynamics. + %% Set up desired linear output dynamics + %% Both Symbolic and Non-symbolic if ~isfield(params, 'epsilon_FL') error("Define variable in the params called 'epsilon_FL', which is the rate of RES-CLF"); end From 7d2217b05f1b0352795410f39a1a7afe4e4ff5aa Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Wed, 11 Aug 2021 19:22:56 +0900 Subject: [PATCH 07/49] Rabbit CBF-CLF-Helper version 0 updated --- a.txt | 1 - anim/anim_flat_ground.m | 168 + anim/anim_moving_stone_load.m | 233 + anim/anim_moving_stone_load_compare.m | 350 + anim/bar_percent.m | 64 + anim/coordinateTransformation.m | 15 + anim/even_sample.m | 42 + anim/make_fig_contact_force.m | 41 + anim/make_fig_step_length.m | 30 + anim/make_fig_stick_moving_stone_load.m | 213 + anim/make_plots.m | 433 + anim/make_plots_com.m | 18 + anim/make_plots_quan.m | 32 + anim/make_plots_quan_save_figs.m | 75 + anim/make_stick_fig.m | 170 + anim/plot_contact_force.m | 41 + anim/plot_contact_force_data.m | 50 + anim/plot_foot_traj.m | 22 + anim/plot_stick_fig.m | 200 + anim/plot_swing_foot_trail.m | 226 + anim/plot_swing_foot_trail_full_stick.m | 250 + anim/pt_circle.m | 7 + anim/pt_circle2.m | 13 + anim/pt_line.m | 1 + anim/pt_link.m | 37 + anim/pt_link2.m | 17 + anim/rabbit_model/COM_pos_vel_accel.m | 19 + anim/rabbit_model/LyapRiccatiGenerator.m | 267 + anim/rabbit_model/README.txt | 202 + anim/rabbit_model/bb.m | 26 + anim/rabbit_model/bezier_obj.m | 93 + anim/rabbit_model/bezier_obj_jwg.m | 105 + anim/rabbit_model/bezier_opt.m | 138 + anim/rabbit_model/bezier_opt_jwg.m | 158 + anim/rabbit_model/center_of_mass.m | 32 + anim/rabbit_model/compute_contact_force.m | 17 + anim/rabbit_model/compute_p1_p5.m | 34 + anim/rabbit_model/compute_transition_ctrl.m | 67 + anim/rabbit_model/controlParameters.m | 157 + anim/rabbit_model/control_synth.m | 39 + anim/rabbit_model/dyn_mod_abs.m | 97 + anim/rabbit_model/energy_compute.m | 9 + anim/rabbit_model/fixlength.m | 39 + anim/rabbit_model/gen_zd.m | 370 + anim/rabbit_model/gen_zd_cc.m | 168 + anim/rabbit_model/gen_zd_diff.m | 237 + anim/rabbit_model/gen_zd_diff_cc.m | 360 + anim/rabbit_model/head_height.m | 16 + anim/rabbit_model/hip_pos.m | 18 + anim/rabbit_model/hip_vel.m | 12 + anim/rabbit_model/impact_abs_red.m | 74 + anim/rabbit_model/initialize.m | 16 + anim/rabbit_model/limb_position.m | 61 + anim/rabbit_model/load_all_tests.m | 11 + anim/rabbit_model/main_zd.m | 36 + anim/rabbit_model/miperhr2mpersec.m | 14 + anim/rabbit_model/mpersec2miperhr.m | 14 + anim/rabbit_model/myaa.m | 337 + anim/rabbit_model/plot_ctrl_param_sens.m | 17 + anim/rabbit_model/plot_ctrl_param_sens_cc.m | 17 + anim/rabbit_model/poincare_main.m | 31 + anim/rabbit_model/poincare_zd_compare.m | 85 + anim/rabbit_model/poincare_zd_compare_cc.m | 78 + anim/rabbit_model/poincare_zd_main.m | 31 + anim/rabbit_model/poincare_zd_main_cc.m | 31 + anim/rabbit_model/quiver_erw.m | 106 + anim/rabbit_model/rm_gaps.m | 41 + anim/rabbit_model/sigma.m | 37 + anim/rabbit_model/sigma_vel.m | 29 + anim/rabbit_model/sim_zd.m | 266 + anim/rabbit_model/sim_zd_cc.m | 213 + anim/rabbit_model/sim_zd_opt.m | 92 + anim/rabbit_model/start_of_step_ctrl_sens.m | 147 + anim/rabbit_model/step_length.m | 11 + anim/rabbit_model/swing_foot_height.m | 16 + anim/rabbit_model/swing_foot_velocity.m | 18 + anim/rabbit_model/symb_control.m | 377 + anim/rabbit_model/symb_model.m | 367 + anim/rabbit_model/symb_model_red.m | 638 ++ anim/rabbit_model/theta_obj.m | 16 + anim/rabbit_model/tilefigs.m | 73 + anim/rabbit_model/total_energy_red.m | 47 + anim/rabbit_model/vectfield.m | 33 + anim/rabbit_model/zd.m | 100 + anim/rabbit_model/zd_cc.m | 1539 ++++ anim/rabbit_model/zd_ctrl_sens_plots_cc.m | 125 + anim/rabbit_model/zd_diff.m | 7751 +++++++++++++++++ anim/rabbit_model/zd_diff_cc.m | 801 ++ anim/rabbit_model/zd_lift.m | 138 + anim/rabbit_model/zd_lift_special.m | 21 + anim/rabbit_model/zd_model.mdl | 842 ++ anim/rabbit_model/zd_project.m | 16 + anim/rabbit_model/zd_project_cc.m | 18 + backup.txt | 86 + demos/cart_pole/init_settings_cart_pole.m | 2 + demos/dynsys/@AccBuiltIn/AccBuiltIn.m | 65 - demos/dynsys/@AccSymbolic/AccSymbolic.m | 16 - demos/dynsys/@AccSymbolic/defineCbf.m | 10 - demos/dynsys/@AccSymbolic/defineClf.m | 6 - demos/dynsys/@AccSymbolic/defineSystem.m | 15 - demos/dynsys/@CartPole/CartPole.m | 17 - demos/dynsys/@CartPole/defineClf.m | 40 - demos/dynsys/@CartPole/defineOutput.m | 23 - demos/dynsys/@CartPole/defineSystem.m | 23 - .../@DoubleIntegrator2D/DoubleIntegrator2D.m | 26 - .../dynsys/@DoubleIntegrator2D/defineSystem.m | 12 - demos/dynsys/@DubinsCar/DubinsCar.m | 12 - demos/dynsys/@DubinsCar/defineCbf.m | 13 - demos/dynsys/@DubinsCar/defineClf.m | 6 - .../@InvertedPendulum/InvertedPendulum.m | 5 - demos/dynsys/@InvertedPendulum/defineClf.m | 12 - demos/dynsys/@InvertedPendulum/defineSystem.m | 11 - demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 66 - demos/init_clf_simulation_rabbit.m | 70 +- demos/run_clf_simulation_rabbit.m | 187 +- demos/two_link_simulate.m | 155 + dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 149 +- dynsys/@RabbitBuiltIn/defineSystem.m | 42 + dynsys/@RabbitBuiltIn/updateAllProperty.m | 8 + dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m | 74 + .../FROST/Ce_five_link_walker.mexw64 | Bin 0 -> 69632 bytes dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m | 89 + .../FROST/De_five_link_walker.mexw64 | Bin 0 -> 44544 bytes dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m | 64 + .../FROST/Ge_five_link_walker.mexw64 | Bin 0 -> 35840 bytes .../Rabbit_helper/FROST/J_LeftToe.mexw64 | Bin 0 -> 35328 bytes .../Rabbit_helper/FROST/J_RightToe.mexw64 | Bin 0 -> 35328 bytes .../Rabbit_helper/FROST/dJ_RightToe.mexw64 | Bin 0 -> 36864 bytes dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m | 28 + .../Rabbit_helper/FROST/p_LeftToe.mexw64 | Bin 0 -> 34304 bytes .../Rabbit_helper/FROST/p_RightToe.mexw64 | Bin 0 -> 34304 bytes .../Rabbit_helper/FROST/p_Torso.mexw64 | Bin 0 -> 33280 bytes .../Rabbit_helper/FROST/p_q2_left.mexw64 | Bin 0 -> 34304 bytes .../Rabbit_helper/FROST/p_q2_right.mexw64 | Bin 0 -> 34304 bytes .../Rabbit_helper/FROST/trajectory_data.mat | Bin 0 -> 4460 bytes .../Rabbit_helper/bezier_outputs.m | 8 +- dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m | 8 + dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m | 192 + dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m | 23 + .../Rabbit_helper/gen/dq_pos_gen_v2.m | 21 + dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m | 8 + dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m | 77 + dynsys_helper/Rabbit_helper/gen/gradient_h1.m | 38 + dynsys_helper/Rabbit_helper/gen/gradient_h2.m | 38 + dynsys_helper/Rabbit_helper/gen/hessian_h1.m | 99 + dynsys_helper/Rabbit_helper/gen/hessian_h2.m | 99 + dynsys_helper/Rabbit_helper/gen/psi_gen.m | 11 + dynsys_helper/Rabbit_helper/modelParameters.m | 34 + dynsys_helper/Rabbit_helper/rabbit_event.m | 16 + lib/@CtrlAffineSys/CtrlAffineSys.m | 6 +- lib/@CtrlAffineSys/init_sys.m | 9 +- lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 18 +- lib/@CtrlAffineSysFL/ctrlClfQpFL.m | 1 + lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m | 1 + lib/@CtrlAffineSysFL/init_sys_FL.m | 3 +- us_Vs.mat | Bin 0 -> 329 bytes utils/rollout_feedback_linearization.m | 55 +- 157 files changed, 21653 insertions(+), 604 deletions(-) delete mode 100644 a.txt create mode 100644 anim/anim_flat_ground.m create mode 100644 anim/anim_moving_stone_load.m create mode 100644 anim/anim_moving_stone_load_compare.m create mode 100644 anim/bar_percent.m create mode 100644 anim/coordinateTransformation.m create mode 100644 anim/even_sample.m create mode 100644 anim/make_fig_contact_force.m create mode 100644 anim/make_fig_step_length.m create mode 100644 anim/make_fig_stick_moving_stone_load.m create mode 100644 anim/make_plots.m create mode 100644 anim/make_plots_com.m create mode 100644 anim/make_plots_quan.m create mode 100644 anim/make_plots_quan_save_figs.m create mode 100644 anim/make_stick_fig.m create mode 100644 anim/plot_contact_force.m create mode 100644 anim/plot_contact_force_data.m create mode 100644 anim/plot_foot_traj.m create mode 100644 anim/plot_stick_fig.m create mode 100644 anim/plot_swing_foot_trail.m create mode 100644 anim/plot_swing_foot_trail_full_stick.m create mode 100644 anim/pt_circle.m create mode 100644 anim/pt_circle2.m create mode 100644 anim/pt_line.m create mode 100644 anim/pt_link.m create mode 100644 anim/pt_link2.m create mode 100644 anim/rabbit_model/COM_pos_vel_accel.m create mode 100644 anim/rabbit_model/LyapRiccatiGenerator.m create mode 100644 anim/rabbit_model/README.txt create mode 100644 anim/rabbit_model/bb.m create mode 100644 anim/rabbit_model/bezier_obj.m create mode 100644 anim/rabbit_model/bezier_obj_jwg.m create mode 100644 anim/rabbit_model/bezier_opt.m create mode 100644 anim/rabbit_model/bezier_opt_jwg.m create mode 100644 anim/rabbit_model/center_of_mass.m create mode 100644 anim/rabbit_model/compute_contact_force.m create mode 100644 anim/rabbit_model/compute_p1_p5.m create mode 100644 anim/rabbit_model/compute_transition_ctrl.m create mode 100644 anim/rabbit_model/controlParameters.m create mode 100644 anim/rabbit_model/control_synth.m create mode 100644 anim/rabbit_model/dyn_mod_abs.m create mode 100644 anim/rabbit_model/energy_compute.m create mode 100644 anim/rabbit_model/fixlength.m create mode 100644 anim/rabbit_model/gen_zd.m create mode 100644 anim/rabbit_model/gen_zd_cc.m create mode 100644 anim/rabbit_model/gen_zd_diff.m create mode 100644 anim/rabbit_model/gen_zd_diff_cc.m create mode 100644 anim/rabbit_model/head_height.m create mode 100644 anim/rabbit_model/hip_pos.m create mode 100644 anim/rabbit_model/hip_vel.m create mode 100644 anim/rabbit_model/impact_abs_red.m create mode 100644 anim/rabbit_model/initialize.m create mode 100644 anim/rabbit_model/limb_position.m create mode 100644 anim/rabbit_model/load_all_tests.m create mode 100644 anim/rabbit_model/main_zd.m create mode 100644 anim/rabbit_model/miperhr2mpersec.m create mode 100644 anim/rabbit_model/mpersec2miperhr.m create mode 100644 anim/rabbit_model/myaa.m create mode 100644 anim/rabbit_model/plot_ctrl_param_sens.m create mode 100644 anim/rabbit_model/plot_ctrl_param_sens_cc.m create mode 100644 anim/rabbit_model/poincare_main.m create mode 100644 anim/rabbit_model/poincare_zd_compare.m create mode 100644 anim/rabbit_model/poincare_zd_compare_cc.m create mode 100644 anim/rabbit_model/poincare_zd_main.m create mode 100644 anim/rabbit_model/poincare_zd_main_cc.m create mode 100644 anim/rabbit_model/quiver_erw.m create mode 100644 anim/rabbit_model/rm_gaps.m create mode 100644 anim/rabbit_model/sigma.m create mode 100644 anim/rabbit_model/sigma_vel.m create mode 100644 anim/rabbit_model/sim_zd.m create mode 100644 anim/rabbit_model/sim_zd_cc.m create mode 100644 anim/rabbit_model/sim_zd_opt.m create mode 100644 anim/rabbit_model/start_of_step_ctrl_sens.m create mode 100644 anim/rabbit_model/step_length.m create mode 100644 anim/rabbit_model/swing_foot_height.m create mode 100644 anim/rabbit_model/swing_foot_velocity.m create mode 100644 anim/rabbit_model/symb_control.m create mode 100644 anim/rabbit_model/symb_model.m create mode 100644 anim/rabbit_model/symb_model_red.m create mode 100644 anim/rabbit_model/theta_obj.m create mode 100644 anim/rabbit_model/tilefigs.m create mode 100644 anim/rabbit_model/total_energy_red.m create mode 100644 anim/rabbit_model/vectfield.m create mode 100644 anim/rabbit_model/zd.m create mode 100644 anim/rabbit_model/zd_cc.m create mode 100644 anim/rabbit_model/zd_ctrl_sens_plots_cc.m create mode 100644 anim/rabbit_model/zd_diff.m create mode 100644 anim/rabbit_model/zd_diff_cc.m create mode 100644 anim/rabbit_model/zd_lift.m create mode 100644 anim/rabbit_model/zd_lift_special.m create mode 100644 anim/rabbit_model/zd_model.mdl create mode 100644 anim/rabbit_model/zd_project.m create mode 100644 anim/rabbit_model/zd_project_cc.m create mode 100644 backup.txt delete mode 100644 demos/dynsys/@AccBuiltIn/AccBuiltIn.m delete mode 100644 demos/dynsys/@AccSymbolic/AccSymbolic.m delete mode 100644 demos/dynsys/@AccSymbolic/defineCbf.m delete mode 100644 demos/dynsys/@AccSymbolic/defineClf.m delete mode 100644 demos/dynsys/@AccSymbolic/defineSystem.m delete mode 100644 demos/dynsys/@CartPole/CartPole.m delete mode 100644 demos/dynsys/@CartPole/defineClf.m delete mode 100644 demos/dynsys/@CartPole/defineOutput.m delete mode 100644 demos/dynsys/@CartPole/defineSystem.m delete mode 100644 demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m delete mode 100644 demos/dynsys/@DoubleIntegrator2D/defineSystem.m delete mode 100644 demos/dynsys/@DubinsCar/DubinsCar.m delete mode 100644 demos/dynsys/@DubinsCar/defineCbf.m delete mode 100644 demos/dynsys/@DubinsCar/defineClf.m delete mode 100644 demos/dynsys/@InvertedPendulum/InvertedPendulum.m delete mode 100644 demos/dynsys/@InvertedPendulum/defineClf.m delete mode 100644 demos/dynsys/@InvertedPendulum/defineSystem.m delete mode 100644 demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m create mode 100644 demos/two_link_simulate.m create mode 100644 dynsys/@RabbitBuiltIn/defineSystem.m create mode 100644 dynsys/@RabbitBuiltIn/updateAllProperty.m create mode 100644 dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m create mode 100644 dynsys_helper/Rabbit_helper/FROST/Ce_five_link_walker.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m create mode 100644 dynsys_helper/Rabbit_helper/FROST/De_five_link_walker.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m create mode 100644 dynsys_helper/Rabbit_helper/FROST/Ge_five_link_walker.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/J_LeftToe.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/J_RightToe.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/dJ_RightToe.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m create mode 100644 dynsys_helper/Rabbit_helper/FROST/p_LeftToe.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/p_RightToe.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/p_Torso.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/p_q2_left.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/p_q2_right.mexw64 create mode 100644 dynsys_helper/Rabbit_helper/FROST/trajectory_data.mat rename demos/dynsys/@CartPole/bezier.m => dynsys_helper/Rabbit_helper/bezier_outputs.m (74%) create mode 100644 dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m create mode 100644 dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m create mode 100644 dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m create mode 100644 dynsys_helper/Rabbit_helper/gen/dq_pos_gen_v2.m create mode 100644 dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m create mode 100644 dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m create mode 100644 dynsys_helper/Rabbit_helper/gen/gradient_h1.m create mode 100644 dynsys_helper/Rabbit_helper/gen/gradient_h2.m create mode 100644 dynsys_helper/Rabbit_helper/gen/hessian_h1.m create mode 100644 dynsys_helper/Rabbit_helper/gen/hessian_h2.m create mode 100644 dynsys_helper/Rabbit_helper/gen/psi_gen.m create mode 100644 dynsys_helper/Rabbit_helper/modelParameters.m create mode 100644 dynsys_helper/Rabbit_helper/rabbit_event.m create mode 100644 us_Vs.mat diff --git a/a.txt b/a.txt deleted file mode 100644 index dd457c7..0000000 --- a/a.txt +++ /dev/null @@ -1 +0,0 @@ -wwwwww diff --git a/anim/anim_flat_ground.m b/anim/anim_flat_ground.m new file mode 100644 index 0000000..6e4670f --- /dev/null +++ b/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/anim/anim_moving_stone_load.m b/anim/anim_moving_stone_load.m new file mode 100644 index 0000000..d1e50b6 --- /dev/null +++ b/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/anim/anim_moving_stone_load_compare.m b/anim/anim_moving_stone_load_compare.m new file mode 100644 index 0000000..f06883d --- /dev/null +++ b/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/anim/bar_percent.m b/anim/bar_percent.m new file mode 100644 index 0000000..836a551 --- /dev/null +++ b/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/anim/coordinateTransformation.m b/anim/coordinateTransformation.m new file mode 100644 index 0000000..8edc862 --- /dev/null +++ b/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/anim/even_sample.m b/anim/even_sample.m new file mode 100644 index 0000000..dbdcde2 --- /dev/null +++ b/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/anim/make_fig_contact_force.m b/anim/make_fig_contact_force.m new file mode 100644 index 0000000..2064a82 --- /dev/null +++ b/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/anim/make_fig_step_length.m b/anim/make_fig_step_length.m new file mode 100644 index 0000000..32b0167 --- /dev/null +++ b/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/anim/make_fig_stick_moving_stone_load.m b/anim/make_fig_stick_moving_stone_load.m new file mode 100644 index 0000000..980bee6 --- /dev/null +++ b/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/anim/make_plots.m b/anim/make_plots.m new file mode 100644 index 0000000..69eeeb7 --- /dev/null +++ b/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/anim/make_plots_com.m b/anim/make_plots_com.m new file mode 100644 index 0000000..e06f6bc --- /dev/null +++ b/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/anim/make_plots_quan.m b/anim/make_plots_quan.m new file mode 100644 index 0000000..437cd0b --- /dev/null +++ b/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/anim/make_plots_quan_save_figs.m b/anim/make_plots_quan_save_figs.m new file mode 100644 index 0000000..d394a1c --- /dev/null +++ b/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/anim/make_stick_fig.m b/anim/make_stick_fig.m new file mode 100644 index 0000000..90e87e9 --- /dev/null +++ b/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/anim/plot_contact_force.m b/anim/plot_contact_force.m new file mode 100644 index 0000000..c11a246 --- /dev/null +++ b/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/anim/plot_contact_force_data.m b/anim/plot_contact_force_data.m new file mode 100644 index 0000000..d47d216 --- /dev/null +++ b/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/anim/plot_foot_traj.m b/anim/plot_foot_traj.m new file mode 100644 index 0000000..11e0fac --- /dev/null +++ b/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/anim/plot_stick_fig.m b/anim/plot_stick_fig.m new file mode 100644 index 0000000..0efa040 --- /dev/null +++ b/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/anim/plot_swing_foot_trail.m b/anim/plot_swing_foot_trail.m new file mode 100644 index 0000000..f38828f --- /dev/null +++ b/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/anim/plot_swing_foot_trail_full_stick.m b/anim/plot_swing_foot_trail_full_stick.m new file mode 100644 index 0000000..591b51e --- /dev/null +++ b/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/anim/pt_circle.m b/anim/pt_circle.m new file mode 100644 index 0000000..1533edc --- /dev/null +++ b/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/anim/pt_circle2.m b/anim/pt_circle2.m new file mode 100644 index 0000000..ae67ce5 --- /dev/null +++ b/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/anim/pt_line.m b/anim/pt_line.m new file mode 100644 index 0000000..e62bc6a --- /dev/null +++ b/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/anim/pt_link.m b/anim/pt_link.m new file mode 100644 index 0000000..3f59298 --- /dev/null +++ b/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/anim/pt_link2.m b/anim/pt_link2.m new file mode 100644 index 0000000..2c37733 --- /dev/null +++ b/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/anim/rabbit_model/COM_pos_vel_accel.m b/anim/rabbit_model/COM_pos_vel_accel.m new file mode 100644 index 0000000..0413a2b --- /dev/null +++ b/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/anim/rabbit_model/LyapRiccatiGenerator.m b/anim/rabbit_model/LyapRiccatiGenerator.m new file mode 100644 index 0000000..2daf96f --- /dev/null +++ b/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/anim/rabbit_model/README.txt b/anim/rabbit_model/README.txt new file mode 100644 index 0000000..177c5ed --- /dev/null +++ b/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/anim/rabbit_model/bb.m b/anim/rabbit_model/bb.m new file mode 100644 index 0000000..df66a3c --- /dev/null +++ b/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/anim/rabbit_model/bezier_obj.m b/anim/rabbit_model/bezier_obj.m new file mode 100644 index 0000000..b2b9b8b --- /dev/null +++ b/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/anim/rabbit_model/bezier_obj_jwg.m b/anim/rabbit_model/bezier_obj_jwg.m new file mode 100644 index 0000000..6509d01 --- /dev/null +++ b/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/anim/rabbit_model/bezier_opt.m b/anim/rabbit_model/bezier_opt.m new file mode 100644 index 0000000..c3c2d91 --- /dev/null +++ b/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/anim/rabbit_model/bezier_opt_jwg.m b/anim/rabbit_model/bezier_opt_jwg.m new file mode 100644 index 0000000..211b2c3 --- /dev/null +++ b/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/anim/rabbit_model/center_of_mass.m b/anim/rabbit_model/center_of_mass.m new file mode 100644 index 0000000..57effdf --- /dev/null +++ b/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/anim/rabbit_model/compute_contact_force.m b/anim/rabbit_model/compute_contact_force.m new file mode 100644 index 0000000..74161e1 --- /dev/null +++ b/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/anim/rabbit_model/compute_p1_p5.m b/anim/rabbit_model/compute_p1_p5.m new file mode 100644 index 0000000..a9b0386 --- /dev/null +++ b/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/anim/rabbit_model/compute_transition_ctrl.m b/anim/rabbit_model/compute_transition_ctrl.m new file mode 100644 index 0000000..80cf817 --- /dev/null +++ b/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/anim/rabbit_model/controlParameters.m b/anim/rabbit_model/controlParameters.m new file mode 100644 index 0000000..c39a9ff --- /dev/null +++ b/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/anim/rabbit_model/control_synth.m b/anim/rabbit_model/control_synth.m new file mode 100644 index 0000000..948e53f --- /dev/null +++ b/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/anim/rabbit_model/dyn_mod_abs.m b/anim/rabbit_model/dyn_mod_abs.m new file mode 100644 index 0000000..90cfe68 --- /dev/null +++ b/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/anim/rabbit_model/energy_compute.m b/anim/rabbit_model/energy_compute.m new file mode 100644 index 0000000..4efe945 --- /dev/null +++ b/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/anim/rabbit_model/fixlength.m b/anim/rabbit_model/fixlength.m new file mode 100644 index 0000000..47e7932 --- /dev/null +++ b/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/anim/rabbit_model/gen_zd.m b/anim/rabbit_model/gen_zd.m new file mode 100644 index 0000000..ddbb986 --- /dev/null +++ b/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/anim/rabbit_model/gen_zd_cc.m b/anim/rabbit_model/gen_zd_cc.m new file mode 100644 index 0000000..01c3631 --- /dev/null +++ b/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/anim/rabbit_model/gen_zd_diff.m b/anim/rabbit_model/gen_zd_diff.m new file mode 100644 index 0000000..d4cb1b7 --- /dev/null +++ b/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/anim/rabbit_model/gen_zd_diff_cc.m b/anim/rabbit_model/gen_zd_diff_cc.m new file mode 100644 index 0000000..92e3cdc --- /dev/null +++ b/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/anim/rabbit_model/head_height.m b/anim/rabbit_model/head_height.m new file mode 100644 index 0000000..28dd453 --- /dev/null +++ b/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/anim/rabbit_model/hip_pos.m b/anim/rabbit_model/hip_pos.m new file mode 100644 index 0000000..e1b11e0 --- /dev/null +++ b/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/anim/rabbit_model/hip_vel.m b/anim/rabbit_model/hip_vel.m new file mode 100644 index 0000000..f94f7dd --- /dev/null +++ b/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/anim/rabbit_model/impact_abs_red.m b/anim/rabbit_model/impact_abs_red.m new file mode 100644 index 0000000..767002b --- /dev/null +++ b/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/anim/rabbit_model/initialize.m b/anim/rabbit_model/initialize.m new file mode 100644 index 0000000..33d327d --- /dev/null +++ b/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/anim/rabbit_model/limb_position.m b/anim/rabbit_model/limb_position.m new file mode 100644 index 0000000..bfa293b --- /dev/null +++ b/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/anim/rabbit_model/load_all_tests.m b/anim/rabbit_model/load_all_tests.m new file mode 100644 index 0000000..9524b8d --- /dev/null +++ b/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/anim/rabbit_model/main_zd.m b/anim/rabbit_model/main_zd.m new file mode 100644 index 0000000..13492ad --- /dev/null +++ b/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/anim/rabbit_model/miperhr2mpersec.m b/anim/rabbit_model/miperhr2mpersec.m new file mode 100644 index 0000000..fbb4e57 --- /dev/null +++ b/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/anim/rabbit_model/mpersec2miperhr.m b/anim/rabbit_model/mpersec2miperhr.m new file mode 100644 index 0000000..6efd21a --- /dev/null +++ b/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/anim/rabbit_model/myaa.m b/anim/rabbit_model/myaa.m new file mode 100644 index 0000000..807e6e5 --- /dev/null +++ b/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/anim/rabbit_model/plot_ctrl_param_sens.m b/anim/rabbit_model/plot_ctrl_param_sens.m new file mode 100644 index 0000000..0ae7d4b --- /dev/null +++ b/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/anim/rabbit_model/plot_ctrl_param_sens_cc.m b/anim/rabbit_model/plot_ctrl_param_sens_cc.m new file mode 100644 index 0000000..073c65e --- /dev/null +++ b/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/anim/rabbit_model/poincare_main.m b/anim/rabbit_model/poincare_main.m new file mode 100644 index 0000000..6a1b38e --- /dev/null +++ b/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/anim/rabbit_model/poincare_zd_compare.m b/anim/rabbit_model/poincare_zd_compare.m new file mode 100644 index 0000000..ba1fdee --- /dev/null +++ b/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/anim/rabbit_model/poincare_zd_compare_cc.m b/anim/rabbit_model/poincare_zd_compare_cc.m new file mode 100644 index 0000000..9a64683 --- /dev/null +++ b/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/anim/rabbit_model/poincare_zd_main.m b/anim/rabbit_model/poincare_zd_main.m new file mode 100644 index 0000000..47b3817 --- /dev/null +++ b/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/anim/rabbit_model/poincare_zd_main_cc.m b/anim/rabbit_model/poincare_zd_main_cc.m new file mode 100644 index 0000000..cadfba4 --- /dev/null +++ b/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/anim/rabbit_model/quiver_erw.m b/anim/rabbit_model/quiver_erw.m new file mode 100644 index 0000000..4ea257b --- /dev/null +++ b/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/anim/rabbit_model/rm_gaps.m b/anim/rabbit_model/rm_gaps.m new file mode 100644 index 0000000..d31d224 --- /dev/null +++ b/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/anim/rabbit_model/sigma.m b/anim/rabbit_model/sigma.m new file mode 100644 index 0000000..d950514 --- /dev/null +++ b/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/anim/rabbit_model/sigma_vel.m b/anim/rabbit_model/sigma_vel.m new file mode 100644 index 0000000..c57631b --- /dev/null +++ b/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/anim/rabbit_model/sim_zd.m b/anim/rabbit_model/sim_zd.m new file mode 100644 index 0000000..8c83b6e --- /dev/null +++ b/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/anim/rabbit_model/sim_zd_cc.m b/anim/rabbit_model/sim_zd_cc.m new file mode 100644 index 0000000..69a428b --- /dev/null +++ b/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/anim/rabbit_model/sim_zd_opt.m b/anim/rabbit_model/sim_zd_opt.m new file mode 100644 index 0000000..de1a98d --- /dev/null +++ b/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/anim/rabbit_model/start_of_step_ctrl_sens.m b/anim/rabbit_model/start_of_step_ctrl_sens.m new file mode 100644 index 0000000..c3234c6 --- /dev/null +++ b/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/anim/rabbit_model/step_length.m b/anim/rabbit_model/step_length.m new file mode 100644 index 0000000..e27c994 --- /dev/null +++ b/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/anim/rabbit_model/swing_foot_height.m b/anim/rabbit_model/swing_foot_height.m new file mode 100644 index 0000000..4407984 --- /dev/null +++ b/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/anim/rabbit_model/swing_foot_velocity.m b/anim/rabbit_model/swing_foot_velocity.m new file mode 100644 index 0000000..58c1a8b --- /dev/null +++ b/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/anim/rabbit_model/symb_control.m b/anim/rabbit_model/symb_control.m new file mode 100644 index 0000000..08b4691 --- /dev/null +++ b/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/anim/rabbit_model/symb_model.m b/anim/rabbit_model/symb_model.m new file mode 100644 index 0000000..cf29f50 --- /dev/null +++ b/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/anim/rabbit_model/symb_model_red.m b/anim/rabbit_model/symb_model_red.m new file mode 100644 index 0000000..5e324fb --- /dev/null +++ b/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/anim/rabbit_model/theta_obj.m b/anim/rabbit_model/theta_obj.m new file mode 100644 index 0000000..51cc4aa --- /dev/null +++ b/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/anim/rabbit_model/tilefigs.m b/anim/rabbit_model/tilefigs.m new file mode 100644 index 0000000..1c13432 --- /dev/null +++ b/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/anim/rabbit_model/total_energy_red.m b/anim/rabbit_model/total_energy_red.m new file mode 100644 index 0000000..8e37aa9 --- /dev/null +++ b/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/anim/rabbit_model/vectfield.m b/anim/rabbit_model/vectfield.m new file mode 100644 index 0000000..4fed74b --- /dev/null +++ b/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/anim/rabbit_model/zd.m b/anim/rabbit_model/zd.m new file mode 100644 index 0000000..6431818 --- /dev/null +++ b/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/anim/rabbit_model/zd_cc.m b/anim/rabbit_model/zd_cc.m new file mode 100644 index 0000000..003f839 --- /dev/null +++ b/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/anim/rabbit_model/zd_ctrl_sens_plots_cc.m b/anim/rabbit_model/zd_ctrl_sens_plots_cc.m new file mode 100644 index 0000000..2026271 --- /dev/null +++ b/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/anim/rabbit_model/zd_diff.m b/anim/rabbit_model/zd_diff.m new file mode 100644 index 0000000..63c8d30 --- /dev/null +++ b/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/anim/rabbit_model/zd_diff_cc.m b/anim/rabbit_model/zd_diff_cc.m new file mode 100644 index 0000000..6cbbd90 --- /dev/null +++ b/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/anim/rabbit_model/zd_lift.m b/anim/rabbit_model/zd_lift.m new file mode 100644 index 0000000..a708d7d --- /dev/null +++ b/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/anim/rabbit_model/zd_lift_special.m b/anim/rabbit_model/zd_lift_special.m new file mode 100644 index 0000000..d8919c0 --- /dev/null +++ b/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/anim/rabbit_model/zd_model.mdl b/anim/rabbit_model/zd_model.mdl new file mode 100644 index 0000000..37037ac --- /dev/null +++ b/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/anim/rabbit_model/zd_project.m b/anim/rabbit_model/zd_project.m new file mode 100644 index 0000000..f75903d --- /dev/null +++ b/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/anim/rabbit_model/zd_project_cc.m b/anim/rabbit_model/zd_project_cc.m new file mode 100644 index 0000000..21fd032 --- /dev/null +++ b/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/backup.txt b/backup.txt new file mode 100644 index 0000000..ce1f409 --- /dev/null +++ b/backup.txt @@ -0,0 +1,86 @@ +for k = 1:total_k-1 + t + % Determine control input. + % dV_hat: analytic Vdot based on model. + [u, slack, V] = controller(x); +% [u, slack, h, V] = controller(s, u_prev); % optimizing the difference between the previous timestep. + 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); + u_prev = u; + t = t + dt; +end + +plot_results(ts, xs, us, params.p_o, params.r_o) + +%% Auxilary Functions +function plot_results(t, xs, us, p_o, r_o) + +figure +subplot(5,1,1) +plot(t, xs(:,1)) +xlabel('t') +ylabel('x [m]') + +subplot(5,1,2) +plot(t, xs(:,2)) +xlabel('t') +ylabel('v_x [m/s]') + +subplot(5,1,3) +plot(t, xs(:,3)) +xlabel('t') +ylabel('y [m]') + +subplot(5,1,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)) +xlabel('t') +ylabel('v [m/s]') + + +figure +subplot(2,1,1) +plot(t(1:end-1), us(:,1)) +xlabel('t') +ylabel('a_x [m/s^2]') + +subplot(2,1,2) +plot(t(1:end-1), 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([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)); +draw_circle(p_o, r_o); + +xlim([lim_min, lim_max]); +ylim([lim_min, lim_max]); +xlabel('x [m]') +ylabel('y [m]') +end + +function h = draw_circle(center,r) +hold on +th = 0:pi/50:2*pi; +xunit = r * cos(th) + center(1); +yunit = r * sin(th) + center(2); +h = plot(xunit, yunit); +hold off +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 index d8efb3f..5f43e76 100644 --- a/demos/cart_pole/init_settings_cart_pole.m +++ b/demos/cart_pole/init_settings_cart_pole.m @@ -23,6 +23,8 @@ 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. diff --git a/demos/dynsys/@AccBuiltIn/AccBuiltIn.m b/demos/dynsys/@AccBuiltIn/AccBuiltIn.m deleted file mode 100644 index 97e21b9..0000000 --- a/demos/dynsys/@AccBuiltIn/AccBuiltIn.m +++ /dev/null @@ -1,65 +0,0 @@ -%% Main Reference -% Aaron Ames et al. Control Barrier Function based Quadratic Programs -% with Application to Adaptive Cruise Control, CDC 2014, Table 1. - -classdef AccBuiltIn < CtrlAffineSys - methods - function obj = AccBuiltIn(params) - % Always using built-in option for setup. - obj = obj@CtrlAffineSys(params, 'built-in'); - end - function Fr = getFr(obj, x) - v = x(2, :); - Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v.^2; - end - function f_ = f(obj, x) - v = x(2, :); - Fr = getFr(obj, x); - f_ = [v; -Fr/obj.params.m; obj.params.v0-v]; - end - function g_ = g(obj, x) - g_ = [0; 1/obj.params.m; 0]; - end - function V = clf(obj, x) - v = x(2, :); - V = (v - obj.params.vd).^2; - end - function LfV = lf_clf(obj, x) - v = x(2, :); - Fr = getFr(obj, x); - LfV = - 2 * Fr .* (v - obj.params.vd) / obj.params.m; - end - function LgV = lg_clf(obj, x) - v = x(2, :); - LgV = 2 * (v - obj.params.vd) / obj.params.m; - end - function B = cbf(obj, x) - v = x(2); - z = x(3); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); - end - function LfB = lf_cbf(obj, x) - v = x(2); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - m = obj.params.m; - Fr = getFr(obj, x); - LfB = (T + (v - v0) / (cd * g)) .* Fr / m + (v0 - v); - end - function LgB = lg_cbf(obj, x) - v = x(2); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - m = obj.params.m; - LgB = -(T + (v - v0) / (cd * g)) / m; - end - end -end diff --git a/demos/dynsys/@AccSymbolic/AccSymbolic.m b/demos/dynsys/@AccSymbolic/AccSymbolic.m deleted file mode 100644 index bce030b..0000000 --- a/demos/dynsys/@AccSymbolic/AccSymbolic.m +++ /dev/null @@ -1,16 +0,0 @@ -%% Main Reference -% Aaron Ames et al. Control Barrier Function based Quadratic Programs -% with Application to Adaptive Cruise Control, CDC 2014, Table 1. - -classdef AccSymbolic < CtrlAffineSys - methods - function obj = AccSymbolic(params) - % Always using symbolic option for setup. - obj = obj@CtrlAffineSys(params, 'symbolic'); - end - function Fr = getFr(obj, x) - v = x(2); - Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v^2; - end - end -end diff --git a/demos/dynsys/@AccSymbolic/defineCbf.m b/demos/dynsys/@AccSymbolic/defineCbf.m deleted file mode 100644 index a6edb99..0000000 --- a/demos/dynsys/@AccSymbolic/defineCbf.m +++ /dev/null @@ -1,10 +0,0 @@ -function cbf = defineCbf(obj, params, symbolic_state) - v = symbolic_state(2); - z = symbolic_state(3); - - v0 = params.v0; - T = params.T; - cd = params.cd; - - cbf = z - T * v - 0.5 * (v0-v)^2 / (cd * params.g); -end \ No newline at end of file diff --git a/demos/dynsys/@AccSymbolic/defineClf.m b/demos/dynsys/@AccSymbolic/defineClf.m deleted file mode 100644 index b2feb92..0000000 --- a/demos/dynsys/@AccSymbolic/defineClf.m +++ /dev/null @@ -1,6 +0,0 @@ -function clf = defineClf(obj, params, symbolic_state) - v = symbolic_state(2); - vd = params.vd; % desired velocity. - - clf = (v - vd)^2; -end \ No newline at end of file diff --git a/demos/dynsys/@AccSymbolic/defineSystem.m b/demos/dynsys/@AccSymbolic/defineSystem.m deleted file mode 100644 index e9c9ad4..0000000 --- a/demos/dynsys/@AccSymbolic/defineSystem.m +++ /dev/null @@ -1,15 +0,0 @@ -function [x, f, g] = defineSystem(~, params) - syms p v z % states - x = [p; v; z]; - - f0 = params.f0; - f1 = params.f1; - f2 = params.f2; - v0 = params.v0; - m = params.m; - Fr = f0 + f1 * v + f2 * v^2; - - % Dynamics - f = [v; -Fr/m; v0-v]; - g = [0; 1/m; 0]; -end \ No newline at end of file diff --git a/demos/dynsys/@CartPole/CartPole.m b/demos/dynsys/@CartPole/CartPole.m deleted file mode 100644 index 42ca739..0000000 --- a/demos/dynsys/@CartPole/CartPole.m +++ /dev/null @@ -1,17 +0,0 @@ -classdef CartPole < CtrlAffineSysFL - methods - function E = getEnergy(obj, s) - l = obj.params.l; % [m] length of pendulum - m = obj.params.m; % [kg] mass of pendulum - M = obj.params.M; % [kg] mass of cart - b = obj.params.b; % [N/m/s] coefficient of friction between cart and ground - g = obj.params.g; % [m/s^2] acceleration of gravity - - % kinetic energy - T = 0.5 * (M+m) * s(2)^2 + m * s(2) * s(4) * l * cos(s(3)) + 0.5 * m * l^2 * s(4)^2; - % potential energy - U = -m * g * l * cos(s(3)); - E = T + U; - end - end -end diff --git a/demos/dynsys/@CartPole/defineClf.m b/demos/dynsys/@CartPole/defineClf.m deleted file mode 100644 index 4a6ad3f..0000000 --- a/demos/dynsys/@CartPole/defineClf.m +++ /dev/null @@ -1,40 +0,0 @@ -function clf = defineClf(obj, params, symbolic_state) - s = symbolic_state; - E = getEnergy(obj, s); - E_ref = params.m * params.g * params.l; - - l = obj.params.l; % [m] length of pendulum - m = obj.params.m; % [kg] mass of pendulum - M = obj.params.M; % [kg] mass of cart - b = obj.params.b; % [N/m/s] coefficient of friction between cart and ground - g = obj.params.g; % [m/s^2] acceleration of gravity - A = [0, 1, 0, 0; - 0, 0, -m*g/M, 0; - 0, 0, 0, 1; - 0, 0, (m+M)*g/M, 0]; - B = [0; 1/M; 0; -1/(M*l)]; - Q = eye(4); - Q(1, 1) = 0.1; - Q(2, 2) = 0.1; - Q(3, 3) = 100; - Q(4, 4) = 20; - R = 0.1; - [K, P] = lqr(A, B, Q, R); - disp(P) - disp(eig(A-B*K)) - s_n = s; - s_n(3) = sin(s(3)); - s_n(4) = -s(4); - V_neighbor = s_n' * P * s_n; - - % clf = (1-cos(s(3))) * V_neighbor; - % clf = V_neighbor; - % clf = (E - E_ref)^2; - % clf = (E - E_ref)^2 + 5 * V_neighbor; - clf = (E - E_ref)^2 + 30 * (1-cos(s(3))) * V_neighbor; - % clf = abs(E - E_ref) * V_neighbor; - - % clf = (E - E_ref)^2 + 20 * exp(-(E - E_ref)^2) * V_neighbor; - - % clf = V_neighbor; -end \ No newline at end of file diff --git a/demos/dynsys/@CartPole/defineOutput.m b/demos/dynsys/@CartPole/defineOutput.m deleted file mode 100644 index a3ffca8..0000000 --- a/demos/dynsys/@CartPole/defineOutput.m +++ /dev/null @@ -1,23 +0,0 @@ -function [y, phase, y_max_exceed, y_min_exceed] = defineOutput(obj, params, symbolic_state) - x = symbolic_state(1); - theta = symbolic_state(3); -% y = params.l * sin(theta); - phase = x + 0.5 * theta; - c_normal = 2.37079632679490; % normalization constant. - phase = phase / c_normal; - - y_desired = obj.bezier(params.bezier, phase); - y_desired_max = obj.bezier(params.bezier, params.phase_max); - y_desired_min = obj.bezier(params.bezier, params.phase_min); -% M = length(params.bezier)-1; % Degree of the bezier curve -% y_desired = sym(0); -% for i = 0:M -% y_desired = y_desired + ... -% params.bezier(i+1) * nchoosek(M, i) * (1-phase_var)^(M-i) * phase_var^i; -% end - - y_actual = theta; - y = y_actual - y_desired; - y_max_exceed = y_actual - y_desired_max; - y_min_exceed = y_actual - y_desired_min; -end \ No newline at end of file diff --git a/demos/dynsys/@CartPole/defineSystem.m b/demos/dynsys/@CartPole/defineSystem.m deleted file mode 100644 index a53dece..0000000 --- a/demos/dynsys/@CartPole/defineSystem.m +++ /dev/null @@ -1,23 +0,0 @@ -function [s, f, g] = defineSystem(obj, params) - -l = params.l; % [m] length of pendulum -m = params.m; % [kg] mass of pendulum -M = params.M; % [kg] mass of cart -b = params.b; % [N/m/s] coefficient of friction between cart and ground -g = params.g; % [m/s^2] acceleration of gravity - -syms x dx theta dtheta -% Theta: upright is pi (-pi) -s = [x; dx; theta; dtheta]; - -f = [s(2); - ( 2*m*l*s(4)^2*sin(s(3)) + 3*m*g*sin(s(3))*cos(s(3)) ... - - 4*b*s(2) )/( 4*(M+m)-3*m*cos(s(3))^2 ); - s(4); - (-3*m*l*s(4)^2*sin(s(3))*cos(s(3)) - 6*(M+m)*g*sin(s(3)) ... - - 6*(-b*s(2))*cos(s(3)) )/( 4*l*(m+M)-3*m*l*cos(s(3))^2 )]; - -g = [0; - 4/( 4*(M+m)-3*m*cos(s(3))^2 ); - 0; - (- 6*cos(s(3)) )/( 4*l*(m+M)-3*m*l*cos(s(3))^2 )]; diff --git a/demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m b/demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m deleted file mode 100644 index 95f4e6d..0000000 --- a/demos/dynsys/@DoubleIntegrator2D/DoubleIntegrator2D.m +++ /dev/null @@ -1,26 +0,0 @@ -classdef DoubleIntegrator2D < CtrlAffineSys - methods - function clf = defineClf(obj, params, symbolic_state) - x = symbolic_state; - - A = zeros(4); - A(1, 2) = 1; - A(3, 4) = 1; - B = [0 0; 1 0; 0 0; 0 1]; - Q = eye(size(A)); - R = eye(size(B,2)); - [~,P] = lqr(A,B,Q,R); - e = x - [params.p_d(1); 0; params.p_d(2); 0]; - clf = e' * P * e; - end - - function cbf = defineCbf(obj, params, symbolic_state) - x = symbolic_state; - p_o = params.p_o; % position of the obstacle. - r_o = params.r_o; % radius of the obstacle. - distance = (x(1) - p_o(1))^2 + (x(3) - p_o(2))^2 - r_o^2; - derivDistance = 2*(x(1)-p_o(1))*x(2) + 2*(x(3)-p_o(2))*x(4); - cbf = derivDistance + params.cbf_gamma0 * distance; - end - end -end diff --git a/demos/dynsys/@DoubleIntegrator2D/defineSystem.m b/demos/dynsys/@DoubleIntegrator2D/defineSystem.m deleted file mode 100644 index 2269459..0000000 --- a/demos/dynsys/@DoubleIntegrator2D/defineSystem.m +++ /dev/null @@ -1,12 +0,0 @@ -function [x, f, g] = defineSystem(~, ~) - syms p_x v_x p_y v_y; - x = [p_x; v_x; p_y; v_y]; - - A = zeros(4); - A(1, 2) = 1; - A(3, 4) = 1; - B = [0 0; 1 0; 0 0; 0 1]; - - f = A * x; - g = B; -end \ No newline at end of file diff --git a/demos/dynsys/@DubinsCar/DubinsCar.m b/demos/dynsys/@DubinsCar/DubinsCar.m deleted file mode 100644 index 46aa6b8..0000000 --- a/demos/dynsys/@DubinsCar/DubinsCar.m +++ /dev/null @@ -1,12 +0,0 @@ -classdef DubinsCar < CtrlAffineSys - methods - function [x, f, g] = defineSystem(obj, params) - syms p_x p_y theta - x = [p_x; p_y; theta]; - f = [params.v * cos(theta); - params.v * sin(theta); - 0]; - g = [0; 0; 1]; - end - end -end \ No newline at end of file diff --git a/demos/dynsys/@DubinsCar/defineCbf.m b/demos/dynsys/@DubinsCar/defineCbf.m deleted file mode 100644 index 78c3342..0000000 --- a/demos/dynsys/@DubinsCar/defineCbf.m +++ /dev/null @@ -1,13 +0,0 @@ -function cbf = defineCbf(~, params, symbolic_state) - x = symbolic_state; - p_x = x(1); p_y = x(2); theta = x(3); - - v = params.v; - xo = params.xo; - yo = params.yo; - d = params.d; - - distance = (p_x - xo)^2 + (p_y - yo)^2 - d^2; - derivDistance = 2*(p_x-xo)*v*cos(theta) + 2*(p_y-yo)*sin(theta); - cbf = derivDistance + params.cbf_gamma0 * distance; -end \ No newline at end of file diff --git a/demos/dynsys/@DubinsCar/defineClf.m b/demos/dynsys/@DubinsCar/defineClf.m deleted file mode 100644 index 3244a93..0000000 --- a/demos/dynsys/@DubinsCar/defineClf.m +++ /dev/null @@ -1,6 +0,0 @@ -function clf = defineClf(~, params, symbolic_state) - p_x = symbolic_state(1); - p_y = symbolic_state(2); - theta = symbolic_state(3); - clf = (cos(theta).*(p_y-params.yd)-sin(theta).*(p_x-params.xd)).^2; -end \ No newline at end of file diff --git a/demos/dynsys/@InvertedPendulum/InvertedPendulum.m b/demos/dynsys/@InvertedPendulum/InvertedPendulum.m deleted file mode 100644 index 286d96a..0000000 --- a/demos/dynsys/@InvertedPendulum/InvertedPendulum.m +++ /dev/null @@ -1,5 +0,0 @@ -classdef InvertedPendulum < CtrlAffineSys - methods - - end -end diff --git a/demos/dynsys/@InvertedPendulum/defineClf.m b/demos/dynsys/@InvertedPendulum/defineClf.m deleted file mode 100644 index cc3a122..0000000 --- a/demos/dynsys/@InvertedPendulum/defineClf.m +++ /dev/null @@ -1,12 +0,0 @@ -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; -end - diff --git a/demos/dynsys/@InvertedPendulum/defineSystem.m b/demos/dynsys/@InvertedPendulum/defineSystem.m deleted file mode 100644 index bbb1359..0000000 --- a/demos/dynsys/@InvertedPendulum/defineSystem.m +++ /dev/null @@ -1,11 +0,0 @@ -function [x, f, g] = defineSystem(obj, params) - syms theta dtheta - x = [theta; dtheta]; - l = params.l; % [m] length of pendulum - m = params.m; % [kg] mass of pendulum - g = params.g; % [m/s^2] acceleration of gravity - b = params.b; % [s*Nm/rad] friction coefficient - - f = [x(2); (-b*x(2) + m*g*l*sin(x(1))/2 ) / (m*l^2/3)]; - g = [0; -1 / (m*l^2/3)]; -end \ No newline at end of file diff --git a/demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m deleted file mode 100644 index 3638e73..0000000 --- a/demos/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ /dev/null @@ -1,66 +0,0 @@ -%% Main Reference -% Aaron Ames et al. Control Barrier Function based Quadratic Programs -% with Application to Adaptive Cruise Control, CDC 2014, Table 1. - -classdef RabbitBuiltIn < CtrlAffineSys - methods - function obj = RabbitBuiltIn(params) - % Always using built-in option for setup. - obj = obj@CtrlAffineSys(params, 'built-in'); - end - function Fr = getFr(obj, x) - v = x(2, :); - Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v.^2; - end - function f_ = f(obj, x) - v = x(2, :); - Fr = getFr(obj, x); - f_ = [v; -Fr/obj.params.m; obj.params.v0-v]; - end - function g_ = g(obj, x) - g_ = [0; 1/obj.params.m; 0]; - end - function V = clf(obj, x) - v = x(2, :); - V = (v - obj.params.vd).^2; - end - function LfV = lf_clf(obj, x) - v = x(2, :); - Fr = getFr(obj, x); - LfV = - 2 * Fr .* (v - obj.params.vd) / obj.params.m; - end - function LgV = lg_clf(obj, x) - v = x(2, :); - LgV = 2 * (v - obj.params.vd) / obj.params.m; - end - function B = cbf(obj, x) - v = x(2); - z = x(3); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); - end - function LfB = lf_cbf(obj, x) - v = x(2); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - m = obj.params.m; - Fr = getFr(obj, x); - LfB = (T + (v - v0) / (cd * g)) .* Fr / m + (v0 - v); - end - function LgB = lg_cbf(obj, x) - v = x(2); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - m = obj.params.m; - LgB = -(T + (v - v0) / (cd * g)) / m; - end - end -end - diff --git a/demos/init_clf_simulation_rabbit.m b/demos/init_clf_simulation_rabbit.m index d093131..e74ccc1 100644 --- a/demos/init_clf_simulation_rabbit.m +++ b/demos/init_clf_simulation_rabbit.m @@ -1,19 +1,55 @@ %% Initialize Global Simulation Configuration of Rabbit -global scale -global a_bez theta_end theta_init -global mu -global uSat -global apply_rl_adaptation -global control_type -global motor_dynamics +function dict = init_clf_simulation_rabbit + %% Aggregate simulation setting in the params.config + % TODO: we should classify some notions into certain groups, mixed ugly + % now! + + %% Control Mode: You can use various control options + dict.mode.motor_dynamics = 1; % going to be supported + dict.mode.apply_rl_adaptation = 0; % going to be supported + % 0: Original(Westervelt) 1: CLF-QP 2: L1-CLF-QP 3: RL-CLF-QP + % IO-RL+CLF: apply_rl_adaptation=0, control_type=1 + dict.mode.control_type = 1; % going to be supported + + %% Physical Property + dict.physics.mu = 0.8; % friction constraint; + + %% Legacy : Trajectory optimization legacy, Initial setting + % This is derived from the code Rabbit-IO-RL code + % This is delicate, keep it fixed + dict.legacy.theta_end = 3.384545423940834; + dict.legacy.theta_init = 2.903323878938605; + dict.legacy.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]; + dict.legacy.q0_ini = [-0.161463255651884;0.739782734886561;0.152681626367748;2.44507104015569;0.658106151373543;2.95111227794945;0.599243461678844]; + dict.legacy.dq0_ini = [0.781190436522269;0.243063912942872;0.247793330530586;1.10287289727466;-0.548512132037030;0.203061891957879;1.50818729243256]; + + %% Fulfill params + dict.xdim = length(dict.legacy.q0_ini) + length(dict.legacy.dq0_ini); + dict.udim = 4; % TODO hard-coding + dict.rel_deg_y = 2; + dict.use_phase = false; + dict.scale = 1; + + %% Input options + dict.uSat = 150; + dict.u_max = dict.uSat; + dict.u_min = -dict.uSat; -motor_dynamics = 1; -apply_rl_adaptation = 0; -control_type = 1; % 0: Original(Westervelt) 1: CLF-QP 2: L1-CLF-QP 3: RL-CLF-QP -% IO-RL+CLF: apply_rl_adaptation=0, control_type=1 -uSat = 150; -mu = 0.8; % friction constraint; -theta_end = 3.384545423940834; -theta_init = 2.903323878938605; -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]; -scale = 2; \ No newline at end of file + %% Control Barrier Function Parameters + dict.cbf.rate = 3; + dict.cbf_gamma0 = 1; + + %% Control Lyapunov Function Parameters + dict.clf.rate = 0.7; + dict.clf.use_user_defined_rate = false; % set this to true if you don't want to use + + dict.epsilon_FL = 0.045; % tune this for controlling convergence rate + dict.Kp_FL = 1; + dict.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 + dict.weight.input = 1; % A (can be scalar or matrix) + dict.weight.slack = 100; % B (should be scalar) +end \ No newline at end of file diff --git a/demos/run_clf_simulation_rabbit.m b/demos/run_clf_simulation_rabbit.m index ae0596b..5384bc2 100644 --- a/demos/run_clf_simulation_rabbit.m +++ b/demos/run_clf_simulation_rabbit.m @@ -1,19 +1,29 @@ %% RABBIT SIMULATION -close all; -clear all; - -%% Set simulation config: global variable -init_clf_simulation_rabbit; % initialize global variables -dt = 0.02; % simulation interval -sim_t = 30; % simulation time -nstep = 5; % number of steps +% This is CBF-CLF Helper Version of RABBIT Bipedal Walking. +% This version now supports +close all; clear all; clc; +%% Set Parameters +params = init_clf_simulation_rabbit; % save + +% Simulation Setting + % sim_t: overall simulation time: do not need it in this mode actually + % dt: time interval to collect data used for plot/animation + % nstep: number of step that RABBIT would take +dt = 0.025; % simulation interval +sim_t = 100; % simulation time +nstep = 10; % number of steps + +with_slack = true; % use slack variable in QP (change this into false in order not to use it) +verbose = false; % show the QP-solving process %% Initialize state % Init state. -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]; + +q0_ini = params.legacy.q0_ini; +dq0_ini = params.legacy.dq0_ini; x0 = [q0_ini;dq0_ini]; -% Rellabelling matrix + +% Relabeling matrix (reset map) R = [1, 0, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0, 0; 0, 0, 1, 0, 0, 0, 0; @@ -21,141 +31,54 @@ 0, 0, 0, 0, 0, 0, 1; 0, 0, 0, 1, 0, 0, 0; 0, 0, 0, 0, 1, 0, 0]; -%% Fullfill params -params.xdim = length(x0); -params.udim = 4; % TODO hard-coding -% Target position -params.p_d = [10; 0]; -% obstacle center -params.p_o = [5; 2]; -% obstacle radius. -params.r_o = 2; - -params.cbf_gamma0 = 1; - -params.u_max = 4; -params.u_min = -4; - -params.clf.rate = 0.7; -params.cbf.rate = 3; - -params.weight.slack = 1; -params.weight.input = 5; - - %% Prepare System -dynsys = RabbitBuiltIn(params); -odeFun = @dynsys.dynamics; -controller = @dynsys.ctrlClfQp; -odeSolver = @ode45; +control_sys = RabbitBuiltIn(params); +plant_sys = RabbitBuiltIn(params); +controller = @control_sys.ctrlClfQpFL; +event_func = @rabbit_event; +%odeFun = @plant_sys.dynamics; +%odeSolver = @ode45; +%options = odeset('Events', @rabbit_event); %% Prepare record paper total_k = ceil(sim_t / dt); x = x0; -t = 0; +t0 = 0; % initialize traces. -xs = zeros(total_k, dynsys.xdim); -ts = zeros(total_k, 1); -us = zeros(total_k-1, dynsys.udim); +% xs = zeros(total_k, dynsys.xdim); +xs = []; +ts = []; +us = zeros(total_k-1, control_sys.udim); hs = zeros(total_k-1, 1); Vs = zeros(total_k-1, 1); Fsts = zeros(total_k-1, 2); xs(1, :) = x0'; -ts(1) = t; +ts(1) = t0; u_prev = [0;0]; % Todo -if motor_dynamics - s0_short2 = s0; - s0_motors = zeros(4, 1); - s0 = [s0_short2; s0_motors]; -end - %% Main Simulation -for k = 1:total_k-1 - t - % Determine control input. - % dV_hat: analytic Vdot based on model. - [u, slack, V] = controller(x); -% [u, slack, h, V] = controller(s, u_prev); % optimizing the difference between the previous timestep. - 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); - u_prev = u; - t = t + dt; -end - -plot_results(ts, xs, us, params.p_o, params.r_o) - -%% Auxilary Functions -function plot_results(t, xs, us, p_o, r_o) - -figure -subplot(5,1,1) -plot(t, xs(:,1)) -xlabel('t') -ylabel('x [m]') - -subplot(5,1,2) -plot(t, xs(:,2)) -xlabel('t') -ylabel('v_x [m/s]') - -subplot(5,1,3) -plot(t, xs(:,3)) -xlabel('t') -ylabel('y [m]') - -subplot(5,1,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)) -xlabel('t') -ylabel('v [m/s]') - - -figure -subplot(2,1,1) -plot(t(1:end-1), us(:,1)) -xlabel('t') -ylabel('a_x [m/s^2]') - -subplot(2,1,2) -plot(t(1:end-1), 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([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)); -draw_circle(p_o, r_o); - -xlim([lim_min, lim_max]); -ylim([lim_min, lim_max]); -xlabel('x [m]') -ylabel('y [m]') +for k = 1:nstep + step_summary = strcat("[Step]", num2str(k)); + disp(step_summary); + [xs_new, us_new, ts_new, Vs_new, ~, ys_new] = rollout_feedback_linearization( ... + x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, [], verbose, event_func); + + % Reset Map when event is detected + n = params.xdim/2; + q = R * xs_new(end, 1:n)'; + dq = R*dq_pos_gen_v2([xs_new(end, 1:n) xs_new(end, n+1:2*n)], plant_sys.params.scale); + + % Renew the new state + x0 = [q; dq]; + t0 = ts_new(end); + + ts = [ts; ts_new(2:end)]; + xs = [xs; xs_new(2:end, :)]; end -function h = draw_circle(center,r) -hold on -th = 0:pi/50:2*pi; -xunit = r * cos(th) + center(1); -yunit = r * sin(th) + center(2); -h = plot(xunit, yunit); -hold off -end \ No newline at end of file +%% Five Link Animation +animation_dt = 0.05; +x_quan_vec = coordinateTransformation(xs); +anim_flat_ground(ts, x_quan_vec, animation_dt) diff --git a/demos/two_link_simulate.m b/demos/two_link_simulate.m new file mode 100644 index 0000000..de2147b --- /dev/null +++ b/demos/two_link_simulate.m @@ -0,0 +1,155 @@ +function [xRec, fRec, uRec, tRec, xRec_tilde] = two_link_simulate(x0, beta, Nsteps, th1d, sim_option, sim_event_option) +%TWO_LINK_SIMULATE simulates the two link robot for Nsteps number of +%walking steps and with I/O parameters beta and initial condition x0. +% Ayush Agrawal, Jason Choi +% ayush.agrawal@berkeley.edu +% jason.choi@berkeley.edu +% Inputs: x0: initial state of the step in full representation in the original coordinate (8 x 1 vector) +% beta: bezier coefficients that define the reference gait +% Nsteps: number of walking steps to simulate +% th1d: desired value of q1 (phase var) at the end of a step +% sim_option: simulation option. +% 'vanilla': use two_link_dynamics.m to run simulation. +% 'tilde': use two_link_dynamics_tilde.m to run simulation. +% 'vanilla_class': use CompassWalker.dynamics to run simulation. +% 'tilde_class': use CompassWalkerTilde.dynamics to run simulation. +% sim_event_option: event used for reset map in simulation. +% 'angle': Reset is defined as q1 reaching q1_desired. +% 'foot': Reset is defined as foot position reaching the +% ground (y=0). + +if nargin<1 || isempty(x0) + x0 = getInitialCondition(); +end +if nargin<3 + Nsteps = 1; +end +if nargin<4 + th1d = -x0(3); +end +if nargin<5 + sim_option = 'vanilla'; +end +if nargin<6 + sim_event_option = 'foot'; +end + +% variables to record data +tRec = 0; +xRec = []; +xRec_tilde = []; +% parameters +params.ctrl_type = 'io'; +params.beta = beta; +params.eps = 0.1; % Epsilon used in feedback linearization. +params.th1d = th1d; +params.reset_q1_threshold = -0.03; + +% Reflection matrix (swap leg1 and 2) +R = two_link_reflect; + +for i = 1:Nsteps + step_summary.x_init = x0; + [xs, ts, xs_tilde] = step_simulate_helper(x0, tRec(end), params, sim_option, sim_event_option); + + % Record data + tRec = [tRec;ts]; + xRec = [xRec;xs]; + xRec_tilde = [xRec_tilde;xs_tilde]; + step_summary.x_terminal = xs(end, :)'; + % Check swing foot position + pSw_at_event = pSw_gen(xs(end, :)'); + step_summary.pSw_terminal = pSw_at_event; + + print_step_summary(i, step_summary); + + %% Apply reset map and reinitialize initial state for next step. + q_minus = xs(end, 1:4)'; + dq_minus = xs(end, 5:end)'; + q_plus = R*q_minus; + dq_plus = R*dqPlus_gen([q_minus;dq_minus]); + x0 = [q_plus;dq_plus]; +end + +tRec = tRec(2:end); +uRec = []; +fRec = []; +%% Compute control inputs and Contact forces +for i = 1:length(tRec) + u = two_link_io_control(xRec(i, :)', params); + uRec = [uRec; u]; + fRec = [fRec; Fst_gen(xRec(i, :)', u)']; +end +end + +function [xs, ts, xs_tilde] = step_simulate_helper(x0, t0, params, sim_option, event_option) +%% function that runs walk step simluation +% Inputs: x0: initial state of the step in full representation in the original coordinate (8 x 1 vector) +% t0: initial time of the step +% params: control params +% .ctrl_type +% .beta +% .eps +% .th1d +% sim_option: simulation option +% 'vanilla': use two_link_dynamics.m to run simulation. +% 'tilde': use two_link_dynamics_tilde.m to run simulation. +% 'vanilla_class': use CompassWalker.dynamics to run simulation. +% 'tilde_class': use CompassWalkerTilde.dynamics to run simulation. + tspan = [t0, t0+3]; + gait_params = [params.beta;params.th1d]; + if strcmp(sim_option, 'vanilla') + dynamics_function = @(t,x)two_link_dynamics(t, x, params); + event_function = @(t, x)two_link_event(t, x, params, event_option); + odeOpts = odeset('Events', event_function); + [ts, xs] = ode45(dynamics_function, tspan, x0, odeOpts); + xs_tilde = xs_tilde_from_original(xs, gait_params); + elseif strcmp(sim_option, 'vanilla_class') + dynsys = CompassWalker(1:4, gait_params, [], event_option); + dynamics_function = @(t,x) dynsys.dynamics(t, x, [], [], true, params); + event_function = @dynsys.reset_event; + odeOpts = odeset('Events', event_function); + x0_condensed = x0([3, 4, 7, 8]); + init_stance_position = pSt_gen(x0); + [ts, xs_condensed] = ode45(dynamics_function, tspan, x0_condensed, odeOpts); + xs = convert_condensed_states_to_full_states( … + xs_condensed, init_stance_position); + xs_tilde = xs_tilde_from_original(xs, gait_params); + elseif strcmp(sim_option, 'tilde') + x0_tilde = [q_tilde_from_q_gen(x0(1:4), gait_params); + dq_tilde_from_qdq_gen(x0(1:4), x0(5:8), gait_params)]; + dynamics_function = @(t,x)two_link_dynamics_tilde(t, x, params); + event_function = @(t, x)two_link_event_tilde(t, x, params, event_option); + odeOpts = odeset('Events', event_function); + [ts, xs_tilde] = ode45(dynamics_function, tspan, x0_tilde, odeOpts); + xs = xs_original_from_tilde(xs_tilde, gait_params); + elseif strcmp(sim_option, 'tilde_class') + dynsys = CompassWalkerTilde(1:4, gait_params, [], event_option); + dynamics_function = @(t,x) dynsys.dynamics(t, x, [], [], true, params); + event_function = @dynsys.reset_event; + odeOpts = odeset('Events', event_function); + init_stance_position = pSt_gen(x0); + x0_tilde = [q_tilde_from_q_gen(x0(1:4), gait_params); + dq_tilde_from_qdq_gen(x0(1:4), x0(5:8), gait_params)]; + x0_condensed_tilde = x0_tilde([3, 4, 7, 8]); + [ts, xs_condensed_tilde] = ode45( … + dynamics_function, tspan, x0_condensed_tilde, odeOpts); + xs_tilde_temp = zeros(length(ts), 8); + xs_tilde_temp(:, [3,4,7,8]) = xs_condensed_tilde; + xs_original_temp = xs_original_from_tilde(xs_tilde_temp, gait_params); + xs_condensed = xs_original_temp(:, [3, 4, 7, 8]); + xs = convert_condensed_states_to_full_states( … + xs_condensed, init_stance_position); + xs_tilde = xs_tilde_from_original(xs, gait_params); + 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'); + disp("Swing foot position at terminal state (x, y)"); + disp(log.pSw_terminal'); +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index 7fe374f..a4fd6b2 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -6,84 +6,151 @@ methods function obj = RabbitBuiltIn(params) % Always using built-in option for setup. - obj = obj@CtrlAffineSysFL(params, 'built-in'); - end - function Fr = getFr(obj, x) - v = x(2, :); - Fr = obj.params.f0 + obj.params.f1 * v + obj.params.f2 * v.^2; + obj = obj@CtrlAffineSysFL(params, 'built-in'); end function f_ = f(obj, x) - v = x(2, :); - Fr = getFr(obj, x); - f_ = [v; -Fr/obj.params.m; obj.params.v0-v]; + % 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.defineSystem(x); end function g_ = g(obj, x) - g_ = [0; 1/obj.params.m; 0]; + % RABBIT g(x) + [~, g_] = obj.defineSystem(x); end function V = clf(obj, x) - v = x(2, :); - V = (v - obj.params.vd).^2; + % TODO: Lyapunov Function: redundant actually + n = obj.xdim/2; + y = obj.y(x); + dy = obj.lf_y(x); + eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; + P = obj.Gram_clf_FL; + % In case of sanity check + if isempty(obj.Gram_clf_FL) + P = eye(obj.udim * 2); % exploiting obj.udim = obj.ydim + end + V = transpose(eta_eps)*P*eta_eps; end function LfV = lf_clf(obj, x) - v = x(2, :); - Fr = getFr(obj, x); - LfV = - 2 * Fr .* (v - obj.params.vd) / obj.params.m; + % TODO: LfV(x) : redundant => CtrlAffineSysFL rehandled + n = obj.xdim/2; + y = obj.y(x); + dy = obj.lf_y(x); + eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; + + P = obj.Gram_clf_FL; + F_FL_eps = obj.F_FL_eps; + + % In case of sanity check + if isempty(obj.F_FL_eps) + F_FL_eps = eye(2 * obj.udim); + P = eye(2 * obj.udim); + end + LfV = transpose(eta_eps)*(transpose(F_FL_eps)*P+P*F_FL_eps)*eta_eps; end function LgV = lg_clf(obj, x) - v = x(2, :); - LgV = 2 * (v - obj.params.vd) / obj.params.m; + % TODO: LgV(x) : redundant => CtrlAffineSysFL rehandled + n = obj.xdim/2; + y = obj.y(x); + dy = obj.lf_y(x); + eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; + + P = obj.Gram_clf_FL; + G_FL = obj.G_FL; + + if isempty(obj.G_FL) % In case of sanity check + G_FL = [zeros(obj.udim); + eye(obj.udim)]; + P = eye(2*obj.udim); + end + + LgV = (2 * (G_FL'*P) * eta_eps)'; end function B = cbf(obj, x) - v = x(2); - z = x(3); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - B = z - T * v - 0.5 * (v0 - v).^2 / (cd * g); + % TODO: CBF(x) end function LfB = lf_cbf(obj, x) - v = x(2); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - m = obj.params.m; - Fr = getFr(obj, x); - LfB = (T + (v - v0) / (cd * g)) .* Fr / m + (v0 - v); + % TODO: LfB(x) end function LgB = lg_cbf(obj, x) - v = x(2); - T = obj.params.T; - cd = obj.params.cd; - v0 = obj.params.v0; - g = obj.params.g; - m = obj.params.m; - LgB = -(T + (v - v0) / (cd * g)) / m; + % TODO: LgB(x) 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; + + a_bez = obj.params.legacy.a_bez; + theta_init = obj.params.legacy.theta_init; + theta_end = obj.params.legacy.theta_end; + phase = min(max((theta - theta_init)/(theta_end - theta_init))); + + yd = bezier_outputs(a_bez, phase); % Not very structured actually + ya = q(4:end); + y_ = ya-yd; end function phase_ = phase(obj, x) - % TODO: Rabbit's phase + % TODO: Rabbit's phase => redundant(?) end function Lfy = lf_y(obj, x) - % TODO: Rabbit's lie derivate of Lfy + % TODO: Not used in RABBIT + n = obj.xdim / 2; + q = x(1:n); + dq = x(n+1: 2*n); + + a_bez = obj.params.legacy.a_bez; + theta_init = obj.params.legacy.theta_init; + theta_end = obj.params.legacy.theta_end; + + dydq_des = dydq_d_gen(q, a_bez, theta_init, theta_end); %[4 x 7] + dydq_act = dydq_a_gen(q); + + Lfy = (dydq_act - dydq_des)*dq; % Use dy = Lfy + Lgyu = Lfy property end function Lgy = lg_y(obj, x) - % TODO: Rabbit's lie derivative of Lgy + % TODO: Not used in RABBIT + Lgy = -1; end function LgLfy = lglf_y(obj, x) % TODO: Rabbit's lie derivative of LgLfy + n = obj.xdim / 2; + q = x(1:n); + + a_bez = obj.params.legacy.a_bez; + theta_init = obj.params.legacy.theta_init; + theta_end = obj.params.legacy.theta_end; + + dydq_des = dydq_d_gen(q, a_bez, theta_init, theta_end); %[4 x 7] + dydq_act = 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); + + a_bez = obj.params.legacy.a_bez; + theta_init = obj.params.legacy.theta_init; + theta_end = obj.params.legacy.theta_end; + + L2ydq_des = L2ydq_d_gen(q, dq, a_bez, theta_init, theta_end); + L2ydq_act = L2ydq_a_gen(q, dq); + f_s = obj.f(x); + + L2fy = (L2ydq_act - L2ydq_des)*f_s; end function y_max_exceed_ = y_max_exceed(obj, x) diff --git a/dynsys/@RabbitBuiltIn/defineSystem.m b/dynsys/@RabbitBuiltIn/defineSystem.m new file mode 100644 index 0000000..181a2a4 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/defineSystem.m @@ -0,0 +1,42 @@ +function [f, g] = defineSystem(obj, x) +% define f and g of the dynamical system +% Forked from RABBIT_IO_RL +% f and g would be calculated - duplicated +% => TODO: should be incorporated + n = obj.xdim / 2; + if obj.params.mode.motor_dynamics + u_s = x(end-3 : end); + end + + q = x(1:n); + dq = x(n+1:2*n); + + Dm = D_gen_rel(q,1); % 7 x 7 + Cm = C_gen_rel(q,dq,1); % 7 x 7 + Gm = G_gen_rel(q,1); + Bm =[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]; + JSt = J_RightToe(q); + JSt = JSt([1,3],:); + dJSt = dJ_RightToe(q,dq); + dJSt = dJSt([1,3],:); + + Hm= Cm*dq + Gm; + % Constraint Force to enforce the holonomic constraint: + FSt_u_m = - pinv(JSt*(Dm\JSt'))*(JSt*(Dm\Bm)); + FSt_nu_m = - pinv(JSt*(Dm\JSt'))*(JSt*(Dm\(-Hm)) + dJSt*dq); + + f_m = [dq; Dm\(-Cm*dq - Gm)]; + g1_m = [zeros(length(dq),obj.udim); Dm\Bm]; + g2_m = [zeros(length(dq),size(FSt_nu_m,1)); Dm\JSt']; + + % dx = f(x)+g(x)u of nominal model + f = f_m + g2_m*(FSt_nu_m); + g = g1_m + g2_m*FSt_u_m; +end + 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/dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m b/dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m new file mode 100644 index 0000000..0812581 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m @@ -0,0 +1,74 @@ +function C = C_gen_rel(q_rel,dq_rel,scale,torso_mass) +if nargin < 4 + torso_mass = 0; +elseif any(isnan(torso_mass)) || isempty(torso_mass) + torso_mass = 0; +end + +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 diff --git a/dynsys_helper/Rabbit_helper/FROST/Ce_five_link_walker.mexw64 b/dynsys_helper/Rabbit_helper/FROST/Ce_five_link_walker.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..cb4ec425ff834e933e244f1d83260533ceaea871 GIT binary patch literal 69632 zcmeHw3w%`7wf7ldAUt9QB^niV#Lz|&8i|i6pc6^-aGThupur1GVlt3u9^+(!L9Go2 z+JqrBD%$v}QCpj8uT8COqqXfIwgjIMt46IFwbu?}Z%5QN+Q)qVz1LcYGdUAT+uQH^ zZto1gIcJ^qUjM!JK4;FH3?-Lz850e|n1pvcZW#U8tCvr2_rDo|_@HARJ;-=?>NCgo zyNaJVc6m*0lQ$4-TpO&c_f}OlG&F|1m-@ZI<_2$VgSYSt7kcX(tNjbo)2C&#>sQ}8 z=)P~XR(ve)uaA8A@ohN%)*F={-J$nYy!U@}vp%l==oajke))lqm3%7nZpZ!`t(71D zNFV#P{?s=rKDtXEU#j;P)K=A~7*bJbkzrJSWva35gqKzeFJ{c~PM>g)VY~#+sl;7A z6MK)QTU~0W=m~}~S+n--4J8GL-+$NzaRm;@NOsO+=A^ldTcF*V=Q6e^pD=|B!|=L{ zNr+>;0xn}V4s!ow82J{{NdzF@YG>!Vj8-+kW4soG{4F8K4*L~0DrHr>Y6jcOi>wv| zt1Cm5hH=Z`5XA640`FJxw!M64n+2q1xK!>Q6r%PY*_1%~3?s52K#CPAH`Jt;ZPS_}-2@o5shwQFjto5+>n^a({)QT9ja(QeLPIKTr+~#mi7RAEX3s zwOOSOrzq++<@96BiQN%p@muBeUA6g7wTY-IufGMG8&!TkRDmK&@Szg?NKxNco4&iT zsZuHrslyKxmHQAj;jdxynE*CNDch||y5}Zru2s}rm0v5AHO{HcKOKSMj>7CLvtxO- z$LuK1PU}y@`W>C{*Ld8VH{73w8E9S}nP`T$WM@IJH^Vc@6+Io&y%{q%W@8cr7V z+;kyn({yd_m)});NK{6eP+_%-hg3FZlWy6@Y~lb0wHm{*`&O|gzm^-QWO0CXqN1Qn z4B(Xjq>BK=fpsed(D}nF9fF@jCMaCHqCCJKnxGRB1-K&4%);zkBr5up1N3t_yP^No zNl6?60rGrFjTGUJI1u$wYJ#P`W}jF&Yc)kgI&erFIE+cHR1+P7UrHvDn^kL>xFQ9( zA`VR2HO3zI-M`_oP&^VOw#wbDVG=kjSceHd_fE1))-V+k)!8UDA|f3mL>vU{A;(e! z9fBVa6N(HI;y`3@Z@vCbLJh=*yq{`tA2ljh9Ru0Tiq(dO6E^qS7NOWQ=V3BvH~uqQrryUFNH0MG;gD?0DB( zx_k+V*p}mnI3`^tp!@v|>s@k0vAkLJLn)lkS->!%euMk%?whZ(sZt+-OALyTOy zTSzV#SwlJ^AxRM-iGz^++LOoqmp9opF)%2q<#gND2*RVH8q*W9wW6gFA8U&y5REv9 zmd}M;FVTo$j~2iOId!rtCKU}X)Np5eKd4Rd%u0IUjXGD$Is2Br`-t?iWM|RY21&zz(xKFEOZf)=DF}G?#U9 z$(15V0m%>tA=4P23JFOkNMb`C<&Zv+Q%cs!Cr0vgkCx-gst?IVZ~Bne*?cxXiOkVn zngPcnEICXh%21a}t3FiJ=UZTv{_w9EOLfwTo+iG1?BPW%22R9KMUkOFzc82d3Emm!&87L%+3%N!l z)`CN80aUy^G>2Qh)zI7_zPs4>8RCH$Fw9}!0g_m)Dnn43I5gIZRVLt(0>C2-%i&L5= zE^+#SOJYNAa>(~y(JAYJgxn*oRLb&yladA`nHmb}Zq=5ENeuW5u;ftkDGW$jSTb8A zmbX?ZfmGm*I53J*2|^${YnrsPzdME)aGF?9iAb7V;+S;dm^g6k)m?1=3`U_K>So8s zNum_e?%1Ht5?weZ4y=b6^h~iPzxK>~IKbm$XFek7kuCxd2LVj(>W9UeNU7!w=vGRe&uxJ+z7=p z0DGDd5s?nkBo5LT<}^>FG=%|cLmXF7rtX!ZLM;as8d7)}kN`y3fv#boKQGo+QB(q~ zTUaFv_g?kfvU<>J?m-Da1OiyC5@751#CljKj`VtT;t0^nDmzF;Wi0OZRQRRkf#nR|?@AIl#Ml(BoSY+Pl8j)4-tY4lO(i*K{2}uM(dUR}rj8I7T z;fba>G9FPewbm-)z|`by&y$SGqQBb1+17Bj3q_U_?H5^qv%OEta1BEzS?hEclaNFp zTd!n0z}a3-rhrb?jS(_(-;G=Adoy4|45Vq`X0|%pou(zK*E9o?G?pw8iRH~h5>Zir ziIE{wi3s@Xl^n9z-%nrQ*6Ps`s0@mVR~(bBs!bf{h-1r&A|TNmc3dclQb@bwT8|#?CxP0+22O5C;LeN#uS}tf>^FXSGp22Y5(S+I3P;q3qzr zM=Ch2Y61a>qbktwdE6iE(!kPLSlE!a3UtoEy^^*wV4sLAt2CLzrsP5e*wZW(5$PaJ z;vkI}r&&U23Io>G`MI4kbzdba)N)XYo+u`+9$^CEZHd%$_bHk zt>j20{iWv^kS+zt%6H}RoB{c)7Q?VzGmuy}Nz(5W!)|5h6~itz{I6%Zb%+7O9+HR$ zAW0mEJcN{7;~<=MqU#l_m~_gpl+Y(4(t*P=4FHG=AxH-yO0^ZnOA2Zgf2N?;N1 zDwG7Wu|h^9B8`6rAvg~MCUy$e~aLd;A!H#m*v|;PWD#tK9=7t@=;f?X>M-z8a+n#K8p$@ zR-XlA9AhFP9gKjp&qxRLmoa`v%ZLtZBNK ztgMW5u7z~%twzg%F|A$d7xzgtAU1mWoG0kuDq)2Ww!9UQ8{o z*6E=~gXc-V@+r=2OWBnAs`0Q@k%W6fLO#qtbkf?Crg_Bf@{C74W`|^ z-oB$_qQ-nF)>4UZclL=&y9{zax6UmhvHX)4_>Ztklc;ow@c>I!ii85H6ePJpQb}K< zE5=;yQGo0oW)ZdZRDG7VX z3JFQNh%_LPNCeKH=D^<&hrCJrS#4=`at(*9z)Go?9%r)$BqZq~0C5nYg#x7CqpdN9 z$~rptO&-G*sYjd>1BR{Q(`stsK-9}gA4EyZRcR|c>*+bb9s6J8ROB9Q2KzylKWLX1 zs#|&x4sl=JAi`1imWEk=g~&(UD2lTDe36sA6+FiBlSEGTBa(cMthfDojO=~ZE|6G# z7Ljr65)tWO1e|?FIw&!~_&vCTM>M8`Bb1e8F0Z#&i;C5Xloq^n^U#%viq(o%Ry~px z>DpV7MD|wfrmQdm(yqfoM1_b+y%Eet`RYzT#NeH&YVo*V{e`Y+l1sO$Qq!Xvl}tv& zm7&K038V?(5n?YC3@!qjf)tv9RcJ@gW@NTcFv?^35Wr+YXTQ zu_Pdp3Y{k;{VcgiBoXpyyYz^#N}i~g5*BR-7XvImMC3K%e2^udJfQ(a*N9|@C2xvk zK*CxxaF|rE5RLXe#JAWag8>M{XTOb99*97+-dDWl9GUOwaGR9~o~kG?m&Jtq} z%a0K`r5M#wU~f*K65`u5XttZBRu!eJQb#Tvv)Mzyx{F0#COwkHl7%9%X0w+iSt4=H z<{Vb}STY+mn{!$IqR6e;oX?VUKpw_mzNBrR z-Z?Dvh;Y>O&XsJ&6UocY?)_Qv4$9){+en_QgS@QL`cv&fRy361W68UT5_O%WyACQ{ zta1~n=$mn91vCVNCJWbzu)@|Sr7T=2LR?3{bE!*Z0po6#pDw}M4bE>+RC-wDrN_8< zbO$UW%#I72JeP@?`l|qSsr4qw7_1@X&RewWDA z6|yYu6)e0?gsL?*X1D6%U|b_>l}|&g)(>3{2#O1W0)VIy9eO?n9CEKj+7BG&bBEmh z6NZoaACeX>atftlJ5SMPMW_t0%EM$_s6P9n6yywHD+|9a!ho&Lb{1YGLIncS5`+g? zeu<<-2tZ{>REAjPkB{>3(&=%8+1^CY0Wr(xHN$Xt?iLjvt8|iQ+bz-x?wt=4B}mI^ z`4(2`k+f7-%NZ1~)Udo&h-x@_v?=iJZ_F5_yE>BR}Tk zt;kv2zDJ8CdA4Ri4$J>amU_M-vu^R6%kpoKy|h16tS0Z`q58ap7}ccRET1RxQBB$- zLrD_~gF+tFz*%D|1f9hpBCj9}5s@FDUCKlGo}gh}=U6%Sa!y~DvU3wBRv*i+6c^4j zO24G-lLW{qQlgU?2s&|rNE01$430-Qp z#IV33Q*bP>NEuNpIBKVOB_ztE0R}m0J;~+Vg$A$!4@uyN)MS-Q*7kBq8G*|=GeBD{rR5mhHEgtt#;=*|{OCuFI>mn0!3WCm~An2S4B1e)W$Kd1&hx{Pz z8czE=4=3PgEdn0SsYB$1qqPWRNkX)mAch4NnSx`1MaqcU%2D&gDWlhNDc!- zr)OJzawIUF$nE8lwY^+YM&K3>e8La7+!_pl+Qhnt(E( z<9pn9g5CtD8l4a z21rt7wN*<`(((ZY3|u1m$9N*)WX8JWX}1?aJ91h zO$k0~F13>!XDUqqH}VLAOhM3m1VPF`Wq={=5U=(Hvx`I4Ia0!v0*t#^UMzBJJlbWz zdsyXo@lRPB0$DG~(R-ReYUC6InS$t*mLp{Zj&Rm5{0EP+6*ww^`#5l)s4$$iX6$E` zPEv6UcZB8jA|Ex}10=_-5KSOc>M;=H8U*-No##9A3zTb2% z&oOa0ENNH^TTD3C8ayJ&kBAjbTm-c+q}d1#O*;@ZlDwDFaR1-|9)F+o)1ZtA5ilB0 z79RFjMgC;U4T&=%;A}j|rm?eM-p|Pm%Nl1bB*Wy`eXpoQMa5c6q7s{Q5t}#|ge(r= z7i;c*y`DfGWA8;G=hoK^4*7_oO)1S&&^2Lv~Y0^Gmh^AQ9o1C@4$bftJzCCP%=!XXzs zQp(ru)yneOBDcn))fx|0`Q%GzoFpqhObTkFvkb;;%*qenk9PjYJ$+@28PQ*xx1J z;QaE__C29zd(vv9dNR+Go=0k1^bNg!s9L{RM2{@3oB_46ZAvvwREKOHZO{DKRA0YS z2j7xyA{+5)dl1T|`jDuqNiV9lhqhQ%B!>EV5YJ5Ie&!^Xa9#Vj>d98ZRu||FQr7we zKo<(0^k*q%vAR@Txh1<5RqxGMg(4NSa#7VQqu4E+h~{|GuNQMK%!}1?gUS}qiwnP0 zyKT4JdeWc1Ljy99k+V&5)|D{GG#1|`r7ik#8d1}wImY4|QB`-eMAi2AqYdEmMK#R^ zaPndMjHuSwRFeQM*DiiNZ>tNRHY$ zqH5Yuv(Jfr&3>e)=Gf|Uq9*Y)^ojL3su{R#96fL^DQ(FfIbby{qFQ#qYDz>^-Ihwo zIia&eqU%hoX4E9WL>Z9L8syQ6-a3x@wv!U(uq~71qtz{{H8$1cqg5}eQQK&Z-Hp1X z6YFzSbHsS6`M8^Bf!CH|@@k$G)fSs-@@no9RdrD)W8$2KBGGkP)d+XW*h}JByJvoch0o|$dWcHHva`jEc%B}p>$*L#eII~kkxLp7dU8|8O zC?a)k*~(8*C{fuKIk#-#zxDwqMw1A?4+!9t=X-C_NIsvD^Q2Uqyq~Tj#Vy%=Hpr8Y z&KE@0W5Y4|=wyp(z^3YKJvBO?+{|M>Xj2_~bf!8+r=J|)Zqfcm$A+2(mZ8A{d})kY zJ-IqCYK4+?sSUN{v+!_H&9&9%L`_C%nxnpO{%dk+>W_qYwau%RIKxrj3FH8u)@%SJsn1c(2RDwhngLS8^`G>fjhwa+8Okk-`^n_>J0Jp#+spB=y|W)m$Zk$oy9M|>VB zVzsdW=A0jD;WI>a*rw`S7?kRZT{^QZ`fj!0>l{a=dcUZu+Zducb^{;e81hMvop4oc~B7Cq;m6ALB-m(wTJBFkd0keDaz&%y}Xm z5UG8nGet^g+);!5+v~XxOnf5KO|ybH2OjhK#Y{)d{p0{&1MRow8zfGJO)YuNUlDkyR7}`1U708I*W|=jbC<2=1tdLi z&2GtM(3XpHnn=wDJ8FI+tU<%AVw(f!INwQXi6{Q#HQy*H(S0-29OH40l9R3cRSL#= z$X4?iBt3A=GbNW1+c=Ni+lM%69^A||x9qp(?~vM-gv*2EHGe}=qEb8w z+g9^ABt3A=M@cSSwwj$&U&eWsqvl_CaLw)et@($fwk5mYcFsy(^JY=CpP-YU%fBb8 zecTCe5MwJp>$q~cdi0U5N4Aq_xqc{Zxw@A}{3D5PhsxRK>b$+TotJ^Atd8&9qNGUE93y*9oY(VLRBtt`fO^+p*mSqRXUPVa;R=4)nfhF zEUxGnu;lxlARy1Ols*;1=UKo$B~a)3tyZ522RH6%{VmVNR1)2zCb!J-PEPaX%ej0me<%N#@1U#7|#Gu%aWmB0q1H zxr5jmD(lk`UAoLB(Sd>a@Jl=mhHY7l4b0pGU}WZgM!2JA?q^2})C6s%ko0JWxUh%Z zcd?Tr_EyU-72{T0zRq!&EoHUa)RK?G2iGtHT{hL^z4-_g8VD_QfH2p&yB=T8j=`=sZWE zZR7w;t}QKRp{pd$piM1#p(lyzkWDptp_4_`KG~BOI$cx)<0|yTghB0KfnSDL3&nrEvt+h8ea+R(g&Q^^;ZE!deX%Pq=_ zJUYcNj7QWang{kH_;7{C_IAP(J}{ddlctHi_Fbb#>aVj20$*_$e{HNZ^A$e6=;_zv ze=BTzqHI*>B=%A2D1|@@ffNEM1X2j35J(}ALLh}e3V{>?DFjjoq!36UkU}7ZKnj5r z0x1Mi2&525A&^2Kg+K~{e|-e-opho8N*RB8hySfRH@f5ye9hSQL=JzO2zha~Cqb)N zJ<6ta4@=ZlXeDSWf1ezj;Lq$>p6xO9*VyFpN1y;g4ndV8tf{uzJE=NzRqYch_(L#}w?%W_X~$E@t)j#b%dC3&wEcP`J) zDxNo3l5zG6{^IcLxgIE$EP7#Y(_w20HoR@bN;+>hV)1yvFDGht^Y4qpunxl-sCf47 z?@Lz1%GtClbb8x+u27EJ9NPA&D>SViPYDpD(MgT*_#St3q#+*1x6&HsW#K`!-+i3Q zV^bvK=56|L3yhp~J^i$)>M%P(3PgOdUT_hDdd=|O;_wG~JEOhuzcJE0B`>l&3V+dy zalCn`y)fJ7`?0HS$t{gwhNjUM&u<}#9qv}JRDRKw+BCE_`ZoA@Xy85UqIapy4r2U5 z(O*-WBQ+!Q*z-|VXZo~KSRaEY7dAzjkB^>3ZoK$f|E=@1{K*o?KLN};+=n0-9#ArT z3*(!giau8#kNY;pGd7(Uhtny_X?VzN%Qr%J`feclEW{flAw2Gmhq|AN{t1W0;a@(g z?3aa~jlU3gM;h=YyapG;gKxw`>KpNEvP}T!E%@U8Mk*Py`{!A>XI-oi_ig?l?Dipk z-fNp8U!If~*;5)H=x{FpC;GOkYx4+D&u?3D95@pM@&-tvPt?WZ9Y@Vjn#;B3bJ1Ub zmAA{c`B_bEL=$WeZ#ShVpQ@*P9OandC(MpAZ*hk&tD_YCVWwSlv2GCGvT!W?DOzOH zuI7x>Gu!J8{#{aXBJ26ZSjfCCs@8BP_aTBVzF0S zN$r`O_ZoUXmw;&jCcc6%mjA<}%;Q?8=2Iw}2GjWh3FeW_Y)`>9uBe=Tg@m z{59ND(Wc8)Ejz+5nc-*RFYqAMsm=>!A7G?*zxJni9BpRxl1KNFiu0-&{s4qcQt=#BAPjI+lKtoBEx9@%_Gw>^VghiW@T)(=dK+1 z&G4kTxhM|>==`rm{&+I!SW94r&9psk6UoiWi^R-~@277CFwOt9kk;Tn*PcIGnGm7< zoJxMCMDuE~ZXX`K4Lht%XU~^2@MNbDCmt9PR%u zEooMc4vzkb4yHuE24c_qNqlOX|ER$laye8I6_WMwvhXKr;-j~AYYR-!!`lAjYDOPF z7kvPJ^aNFX{IYtSs0>dNl_qQG-RDl<*94<*QinRPX-Em6cyLDW)zbNH_k7qz&qp9c zc1KUbkpm}X3yfZZlJ2(_5E8<7A*9}nAGwc#!%aJXqL%uM!tL$u3>-)AS_>eqhdNGp z3c*>?FVVqN`1=-wd*0i;9X`GP?id)AvpSEiFvIU7gfIL`+b6D!uigj$(X-(MbtDz}HLB7@7A91Txk3o@w)!(v5 z$yvqWZ9LW7$6pkWH?KFV+^ay~ynVRd&G2kEUC+Goewuj&S?i1Xn3ZgtlGR0hvH6wgEo;@R%&%ttY>jTQQA1AptxH~p z)Y$w=Xwt@A%?IHEmDd0k3&aB{x}rZOE;mZ`rONX?ls%|S%&$Oh4;KU z|JHe`BzFm7n~SzLH=3O#S@FyUTr`{EqAatc$P>@3!BIG3o}}n%qB~2xbi7I(7vVCm z#A9w7_L_O1^NKuX-Zt|QT+ZPIPsiaPPl7Gl4m7rz9qw1ERFGA;PW$5^4tJ?TOw-ll z9e!QBs#%>dJ!adIFQ12TZJr#@{5JmHYv0wj1rm_^xEev?bFYO0vB7%_yxnI{CXU(Z z-i>3O1L)sbw8HkzDz!SLO^VhLF#-A7mV4;GDEe)4e+rWQ&>GYE-E1FD{N{%D)vxD= zUwPy(HF>{;!0IFte(IUy+vc__BY+vp+f~r7{^!o<_n@m7@yt)I0~NkAsx@v_{}&^A z4iAx{UL}E(#b#S|c0SZQ)qe$PhP6^L&fxf*m0OG}rp`|FUqhU(taYc{)aX_`3)4!7teeaB+d=f{_xt7kz?H(bz|J2fA}9y zLo2lYwx+N~eE$SSLY1YeR1a5oo?R0yUwU$$lxZ9_iF z#?hndYDV>LPwU=o8=0^!W5Xwg?^ag` z7yY_*qYq3JV_Sq#*QI9XQ5Std4b>?Z<0IZ4{5AI?6Bq7S@t%v;BL`eHv~5o_7rhbu z&ruhu4wU$Ot3S8uYhL(1LVtWb<;V+0`=0qX`;O1`--UWq{vP$QI7=41Hh&cBa9(7G zySSV_dS={H7~ZzSeL4F#Kd;cvr!O5Bv?G+JX&xYa9H_3O2+VLjJm< z>4Syg*9yb#Jr#ygu;Igk=1@)1^jE?!6i$C-!`^fK4b6Ew!y|>$cj)Zk`R7`WVq zb4UvyRX@OKbgURgBEMNQa!q#LPMw;{sAL35b*zqNJo?U5?QsHZ(qU7O@#tH#ni_Vv zXCu=Iu;1Z66cpAeEjqI4LZvagsTj^&gXXrkz2@{mvtxw^XADpF-4rxZ@;FuJ$ieU-mcQyck#zt@wr2K`=s7J zrne93ZI6w=Mbp>nZ9s1?)!XHIdyd}b>+K2HKK*o`YrEr}{FB;00CdVUg+K~{6apy( zQV66FNFk6yAca5*ffNEM1X2j35U?Sz@A;MZu#KG}QwXFGNFk6yAca5*ffNEM1X2j3 z5J(}ALLh}e3W0xN1bi#-jFHz2zqh%Bo|4#o<6?RC(~?EqOYl5niGEgm_3kSyg{Pm^ zD)ioec3bWDd3_&cm;lBu%se-EL?QqIgH%V4_?8AHqYC+ zAslN#m^oFhDt7koU?ps&=#yY;}p!pBo z`TO5m$Ddw%*<+!bqSi6l$E;)0|D$!xdH8s5Q2eRZ@wAuk4J?^!{y{W@~g z@jUC8>-)LEFV5FG=6tPVQj1x~oKHmTIUnno^RbROpHav2tYc$8uWcv1v|5SP-_+lU zclBTJ{;lE|hRE-^<)wGEe-Us+jqxj(p|-rsWNsV6T8 zR9`A(;Wu9l^;R$Z3TWz;LLh}e3V{>?|6~MQ_z$){XXD3CBADMn-j-kpY{Gh0_CKv*2F*XdT`+Cb7{mRc^D=j9IXqwS0;(3-+@XdIBoK z)FUPvN1U8t91%Qpcvj$$3i0_o{9XM`!}uY5s=a!-R2~;hD0S&PrdWAsnW|02pcZJl2}+*by0H5zPElGN+}IRef26-4#xo@zwZn19Zjn3DXq=rgOmyVm?l z{R(K}EA{a)5s&|AM}xSQxO9VIbWd~{0qpN+!#oE)c$Z>s!XKUxU>2=-UuIv@u64(xBhbwD9>2C>inmSLO- zx)A#fc(;Ha#{MI`eQuZ0bDv?ng7*OUQS4`b+c2`gKN0%^ygi^z>;rh`g1!d(8}QBt z-HrVxc#pt#Rxe`W?FGLS`)a&%K-XdaL%fHeGl2aKxW0&jeggZ|xW2dp^hWG^@g4;I zF!pcaJq-GN>=!*?7+XPa#r^@jw}9@$e${tz{RRK+*gt@G8R!`H)BnRTDnOrz{S^yD8c>gr-ALRXw zyuX(Bm+`)o_n+bYG~UNfCx5T<{t4dS&-=x!_kSC4Pd?>k@;-<6|ExR$?Ef;}SO1ga z-NXJ5Wt>;>{!`|^!TSrCzkv6TG5s_uqbOi}Nn%*g~B5PThyWB;el#-pI-Oco%>d&2%bJ!psxNq2 zj6Qu-*w}n&o!>C-C9Tq+aWhfp8Ad6nr9ppX$WK}&m7!p5OZjPexwtZ~y|lh%L3Lf7 zk*hwA7X^bQO>37`7c^8aZwYCspTtI*QC_~ZyeZ(Xs$ElCRbErsP>ql7hA|iXrciZx zXnnw6UfZyyvAo*f6bd%3$M;H2A;UNme(Rh3pe|Z^!HV;iFDofJ>Ev7;(&#d(8k>y2 z8iB?uje}iHwGGCp27J_p@D*0QaoRq*+;5y^lsEZn@Qqa+gykyZETg=tv0+W^+UB6Y zyrD7}Y`n4@-|nx&5B`&7EuI9p$`bh-*45&>eD&SGVVs zEs{qZmaLIm7ITbpe~Z7WIph}+86AyWf}wI?3`fRMa#W7T7OF0X37E#A;1adgPzBaD zK;6JykNNdY^RKLJm=6#5{w`EAzwV^@C(UP!M#+QC_~rU~zg;DF()rE);QCU3a7|+n zk2+NOmo`ERgX5Wij#=6q45H@JU}KfPscBiYG0VnXUW3M{2Ge+5aW7nts_T~{Kta4%C}+}L<|b3kqY6*Yu{>+vZ?NiM1lhMFtuRx|(zWOLcXWle?tOPklO^#@CX z_^K0wcB6Ym1GQasQA?FSpza|dNmZzEf%`&#XiV`hbZwzZga|A{i(YI5;GYp`44#9z zflMV%z0xjetZuIJo4U&{HjK{~U2tAeao$4e+rMwcN?D~4_@9HosYvx)PWRuiKMn8c z{APA)_b-eghl0&bA$3Y0r?)(^gJl!0;)1oI z_1cdYuf^(nC`9pBmj-Ls;o@hlzsVBX#YOr85EmcC{&oI3Z=KpMnN!)ctYKXvo}rrK zZLTe-QrB8b=B%l#Yx2)IbMfaa--*VSf18FQ`r^+e78zY{YOM7~?na!0-c6+`BePUB(RT@MT|*+NECq+6ep~#~Lox&JI-fso*C5>QOku8~xXCXx`9d@spj4hj(oiD*fIz!KH~OZOuo)>`5Q z3)#?8YunUDFW1^OxApc3mQVHc98 zwd()r?f;xSXXc&vj?NLbqW3~58WWSjG(t0HuW-}d=63$~R0M50< zy>|+J_cNBKcxS1HAipn`KLBh#(C0Yj2HY*YyU9KjJY?SBlOpDDFjh6! zS6dyZX6*e;2)b}i$Jv52=5S*A%q2BuL5^4tPF~-OGv;ujcXOLbv5dzS!O4Bb1uo{I zbFPQ;pBcd)_H3EObi1Xf{^?D0nV7i$@Ow_YU(yJHiPGO7$?`; zgEQuEay_5lR|B3H3kGGb_oEmkkWQiJZtz0!7#CrTgx({(F?u)|TQS=D`=Ac-RTQM@`IMb&jEezyR{dE9nkE{Xt`dbx5F zhdRiqb`B8b7OOcW^xoq>CM`s91GgyPMb#1%N4dpxt~Z4Pu(>Eg9R8&_DAsa;ohyHv zHxF}wS9x)g+n(kY$9b`k!=L2T7A`o#oxi}F1H5>i7Y{8)@dX}zfCv6*(E6ca21MynJA(hG^OxqMm(84>jbTCfY?1>iriT{a4PuZrpvK;47xZ=-JAzLX- zh(}0K3OVOcSf^a%DTTy%gkLI!s&go$D}@pS?aInIWxLgtRfVv(XK4wdC)TI#;WgNK zqJ$!ZB_vc@<%)yuHY-=w_*fPxSI%=Nw0|h`p(%x=c!Uw9P=5}E>y<*e=#5hZTrUDH zm%VwXP4-3;LMWm)RjN0nh29Vcy=f6h*+LoFfpWb>dRQSjgb>+*uvsCI7LbSoQ7dLi zOp;**W((=Nvd9C0v?>si0)#lw@3PRKT&|o~#W-)5k)`}px6d=R_h^B(!8BtB}dlU{260=MM z)2nbu3LN4N2sUF4#HKIUbgW;QZ%;W7@Bn1d@08*}k$riy-6Z{l1p$R6gDKPB{horzE4n#Ty zW6i%?4pWr~Ka(u1JGOB)42S^(hm<&k1bH9^B-v7OK1tN#mZO}L99YDG1>fq4{t-b7 zANY9^>5*0$RP2OyDm>Bx9&upTtdhd@6+HNw#4A#f;o^t064BKXg-2SzBM$6Z2+x|O z>@Z?rmrJ-t5grVP0ZEmVOrXFO1Cn|vIrC-NC4y>J&Pfg|;=p21C5H&as|oKd66tFS z$tgNR)3;UOkrwcX1H4w19Imh6!Ecv%SF2>h4V)2Pm%<}0;1P%0$>by}I~pX12y&MM zJ+6Yx7NE#GEV~sbX#tct5HSgQH`yr$;)NH?6{zSEx>BjoLLI9zOe(b0$E>n@q-mLo z3c;eQ%>uVaMMYYOia2n&NBFi|58WjOPc6T2NfM}xNa!=9Lf2o+n%b+-i2%B>mZ!)* zHz{;t0DVaHnSXG?g<}1#LU)S55k6c;r0ctskaA74rC<3aE%+r4?1p8p&R2ek0l)3a zZ;JGL`b)Cow6Mmi*hxj@niff*56Gx=GL2u5231s~g{X)FyKWg(mElq_;F2Uz$&^ub z#zLzl5%M90P6W{FMT#LBk?08uoftsZg#;=NLRS+F*24l1#E2qjJiws@fuJ;ipx{6d z4gkApeAAll6oZIN=@*<9`!B4SvMUb>8Nwh2toBH&JZ0t3UppnOa->y*vLXhodZbkX zSJbi+)Dv?%SRaV4+RTW8;Ig(5qi0l-Gb*l=nSul*NyIz9cs^m_R zt3@6V%?h1#fleHVG6@|VWu*xMq873es|fN!w+!%G6&fi3jW`hX&?HP-FFJ_N0!}^Y zT{*PvGS07Wl_?_ztagzk?Q&(M=>x;ks!LjZPFWEHR^2KLGDjH1fyhCfvR<$9+)ABu z@trv2dnD**mApiCzFl>`N1>B0(1`=Ln}T2PreqA_|QZfJPjk^-{{z zJsrbJH}8HKqmMf7;vc9C;ef&;E#MIccsVLMW9I=w@Q27GZEuUbR!j6v|1Fa|D1DtT zWC)xXu+o(yGKVALKon7d4+;XY(T#Ojf;~ZCuB>_yI0;7-9%%uOIKazOfg5-*1V6

*Kd(?iWso6lSG-r5pf{ur@*cKWQ0{q_{o%5uPLln>X*6# zVd;=64=R-*r9zhxRI;VY7nMqvQlV=DDmhZ6MXAtD32CQ!3za;na*a}kOS%f2nrJf1p|Uyf*@cJWDcBu8X0RcxpK*mE+sO8p3N%x zLJvXO1p&))snV!aQk063uqvr?gHqX}`e7ujUaG88D#J>}NLaH}NmME$O2tT6i&S|n zD8ubmDn`OuNd^04K_Caz0T2`>2nq%SxdcJLAjllu9*_x3Qm$HsD@fX95EV)iQ9;l= zfl8NDS*TR#ibh(86v{4AQE+fgMj%K8m_ZrC-GR8#fuvhH7*rC&0gVn+_DGeVDHS73 z8Xc(gs2B(iuE_`liJ&tiWB9XQ#$Zg0Ug@ArIWS@{CPqZ6%u^~x493LhmnvTs`0z}y zAjlX5sUQ|zXpRk$_TECD$g)VpAF{v_9spN^Mp`t64VN$_>2m;}f5eO0i z#x7$hRWS@J>k;W-kCGS;=putL|2~QD;@gyp5oWhSNg)+1_$mf6LJTAVOs0(C@E7EK zF|KdBbdaSS7%>>vccxUaC>0|HG|PiIbVnYKY7yTxR7AfC~Qb_q)oH3AqFH)vQb-Ugxw>?oWqe(+EHPI z2P|EX=-b}rH%xn)l>+OxW%r@EQ)-t}8!){J#wlU$Ghi@|`XUMQAjN9HkhX+D5Sp}J zr>uv#^@nd!5LIKWNqf|~L|SJm>s`DHzmV1|##oc~Xcr=?3*{2#1+SQ!bj9dfB+OC4 zz18RfX^#d`C9S`qtog;HFO=2~jIkzdVXdAjGyd3lNT5tfv?#7+l#8+HrdUVC;^{vb zLEJx9EFSF};=!?E@!Z}J)3O%hTRciP#B{yKh{bbqLrjxAMx1>1$oZ`DHpvVR^BiTo zNyGdd!Ic@tq^Oq}%~#spT>I@eNc+ZEZOly49vzQnX+1$%^R-9+7ipa~#+tN8C&eC- zD&smaW;zICzO+aW|J*2h!(gPZk{+HF+|l_$+N1HcO6wYBy@$s)QCc^Su_ovm~9 zOIfFI-_O2Ixw(3bHE9cLHLp@eM<-^?CXRA3od(9vm()1%&{*-LIPu6>@#Hu$E!{D` z)8fRmGRBC{ixZEV>9#nr5jmf2mIgUKcAldj{+@DlrQoXZNt-O1qY3Vk3Esa+8fS9j z+okbg!BxgPvq*c?dY81mM_Kpq!M#daca5LD7RM+H~qD9taLqY3Vn3ErfPbNKK~k;cI|5bLKi_oX6+PtFMxAew^{nEYgmfGubMXxUs)tl#3yDialb?oS7Xb9(RAgGEO{h zy}l|=Jno{Lqr_vgHuqeDtCc{cZ(OuFF=FdGZaHVPZTkn2ynW-|%qYUE6b@I1D=SmH zSs`Am2%XeIcjC*4K!( zB);q8uc+$(F%`R^(Ee<>tAV^I%O0nNy^qxWcDyd{Rh%Zr+ZwME^L}p_jt{Ue;x7s` zkoE)EhQ;n+qqDU8@HZ^87K!pIQC=cSyC@Sxx%(SD+>_aDM*<5&$1QCu2jJ zzYb0vTKH5^_}$!t`Y!>zJrYdLjU3Z~rw7sAG1%_PaylQkgqHpSPnTIBmG+#8kR7n@ z0gK0{ZxN<}jk*;CHsWch^t}Tpj-voW=k!l`u}m;B3fG`pooVw+VSV`&#@ZsmEA;gQ z|*S>L$o7kRPxvhBJKXVb1x{9nb2go{vVI+oQI&)lq=X=Ahw0EtJOs_<-22 zzYFp9NB|$d@tOHq{SwHF!oPfy+ZTopM_-Iubv$XNNrZ2c4(;X(e{1ha5^ zCt65%_&3P4ei>fsR-(XWgWZ7ei5-esf>&bvFNZAm2sbH#%xJuhH@1z&4j{Kh1J+P< zLU6W$6@iN3Vw%BTZv=HBDfcMmehww31vzo%f4=f#kFp%s27WySvS@I%@Ws#F!e`k? z_>>ktrnManT02)-PT;B8v--E5<9#_0en|^I7kyC6O!?hU=RTzj!! z&XuCD^_xQ0mmeN@!iiLde!u+6XP{7+8;O}uTDULFxtztFw~cQ;&Bl~)bZ)2=(p(S`)gEwtL-?lvdr*y$Lo|uFy zziy$HX}d2rb`Idy!ilqT&^>fO#J?Ny;}iX*hCmBzDJQHNf=kbh3~RQZragtk1mBrY zYjBU{#2<}Fh!8ksgp;hVdlu{Vh5CP>!pe00WWLx166Z%wBz> zV8yR#@+JKUe2$0ZngdPMFN8C`cI%IDXD82sDQpIw!5Y#5m3W1-&EvxG8GhkoZXFXA zxIixm{mBx<~d$rEpixUvxvl48dlp&rk? z#pJmLc6tDg;MuA#L~C50xGi$@9+ZU1cR?r@&NYxaY(KZI0$|(0f9Llpn`>XY^*Xfm zy$6wqe}%ftV7kt2(U=Ileh5z--?0yvj?ax77#=gfkFM3iC*i^wemQi;V*APufUj2p z1pT-Y7yaev%kJC3tHxs>IpBAI*Ufpwf``d?Vxtz$nB&2E5$cbI_tJGXwEW8c9!uei z{YANZi#iI1aOyU@sE4s>DIT^>vE~D2ylhI^LhzN&HG-a!wiGoVni6!#xqjah_==wC zE6_=mtB+En%b7*tK6$BGZ|;pogIl#4>vj;hZ|93@vVGe^aphI^(v_E=87wQha3?eBR(t?d$UjcF87Fr zyAPl0>7hHa*q1n!iVH6Zb%MAKGtV|oXX_WuiybfPkM;BWCPrq`_M^dM?JF5?;o0^0 zAt(LTrBO)Pj+X<8+m8g#!v;iyIjY?U+I4&U1$ra%VE_ z-{(K{yE)&vl6T1(gl}!}zMxm@EY6H(eCr<0ExAwrhPu4lNgSZh>9P?bDvXb`A%SE<+<%Ia|CB+33?ctd=_Nqyqc2KMtVz0iGT6 z1g-yThu`QkzhT<7(9+M8U|fSq(TwNY)P^u_IY#C#<|7!L^#dqS?!cP}-mx2!h@*8{ zr=Trj0G(mbJhzembk-U;R7UxEoxnor0O)%U`O&ogN=K+S)Xeu8DF2H8MA?j z9i`R@34he>)H(}>ZTIctw8ovD36VEvG~<1cxHqJ}e{@FVQtKmbj=fvFs)a8Sc(y*; zq=>&P&e_69@NV&5@-HPiSJdl;G6RliMk!)?-<4Hi=}m=_?eU_lDqBGx;^8*q<|yCI z@avCvEQNOrSx1lf>UN#6mpK)`mwDaT%LE5Qy+{{3Z}an^-VqRN%r;?MA24rw_F^(c zXFYniF_+ z9c>S}!hHv<)oAkRy6xSX{9>0Y{ERDnB!Anx?xuhzf7>Zfle;i~+i+9B-B2)Tz!g5~ z3R|buFqXgVbbc^UUoh$A@QbcVFK;`w%H0&qJs2KwO*$ezh}s_NC>VBy54t*vN5cIb z>)-2G`(XiYd!DWY~19 z9YP>)EFQTtEBBxXjYm{Gf}lFq>$WFfpC}L$U~>&@@@-GPmhNvlV09tV1lS+2E(3*i zN{D9o*Km!C{6zq>3}}77cW9Faw2rlQEnHyFN?MW>oFBH@VG-(2Ibe-eGKNXU-Y?J{ z9o92w1*V1i9S5v$fy9-~I;?Mi;xi`?vl5@%oge)!5Hoo{1}TOPb2c}!(7GKl;TjY( zcrTw*;k0@G#3yRTf~T?Bh-izSQQ|m{(OIXy%%2i{D9U$4S=GuprJ{6+^3S5wMfrg! zcZ>3{D0@WtkSKSG@+YE-YLo?fj@%3o{G*oF3SHBDoLU}nI~fSCa^17-%y444@(Ghk-G%z&8z zGXtMk2I}f;pW)97@wtEK69YAY%nX=~-6MwtXoWdq+`c+C1?VdO>q|E&jZ^xJthq5M6jbBWLUjS%47cxd*r9;jNB{?Y_Qn-&m!; z>l~WFnHexMU}nI~fSCa^17-%y444@(Ghk-G%z&8zGXozp1L9o;voJGYX28sVnE^8c zW(Ld*m>DoL@M&k@rY5(;)6^UcWc#mh_}pIzdVKC$NA~7w-@Gg4!g+CiSyBFqMa66C ztDD`mi#$!6s~bGEtAmY&+{n+$yk19Rb<LW`^M^)hI&8v z+=0X6cWm)_n>IpsoF9O#_6ADb)eY<1H36?rp#A0Iii)r8{X_Os%a!K+-{HqWquqN+ z+liZd4;yWgzihO>nEsPhuif#6(f-2H)Zcus`Aws}?z}mVKJedf8|@G7y8WrZH*}*- zeus_r65Bh6UuisTw0FLA^RE+cK4Y|HoK6)l_qH@O*=SRI6ODFz+ifpC^?It&&K1WD zqs_#TZM0>7pC-O4hmE%M7g3V4)xVhi690WM@we^|pgy!4C%sE(lxF%o_7ya9$$W^tD|&r=At*R zo56oyVdbKVwSKqHUr|?6?GIGAyfw5$RIFXO!tLASZg6j{D0TbY)xMhgidBVW6?HzZ zKQO1(y?IU}4BWm7cT06+bA!9WSG{3_C*YXlsPWIwyRKrRyQ!jfYg2Wjr^a95a#z-Q zHoGeuJWZP_w^TQ5a{J~sx?8qfyI^i}Eq=Cv4*UiS<0n6+QDv592FwhY889>ODQ3We z|K!NN0zW2{YGJ9d^^^oQ#d4HcF0il*?xp~yWTdc+z(kf&o6Is6B(aS7Q-+4|3rqag z{SEA#98UbgC4M4T9Txl=P}2N_k}0+%W~)tMHrUwaPij^+N%@Rr;k^4SHkL#SOXTsS zr?7O`rk5qNbl9iQw>NYAiPMwV^aVCH-FN;_dh@g@h50YQTLZiA1C!VM5OI_ylvqR@ z$wnMPruITLw%CSwZkd#f0mtKo;kfX8cLlVD*9yx(JNWq)&wIxUfQ){V(h0lvp`2szlSpbIx)bLD(A}s#tH1-j8}&8V_lzK(3e>GQuL6H3>c?>|0lgpfv_kY9 zIt{2pICaqNs2AM8*u&7NLLI{S80db~uj70QbmEQpao{5K7xZq_gE;qtK8e~{jQ)de zK>aXI9rRT z6^^at|MCy^&|GUyYNWL^#XUqE2ilABI-!ALgPubr?690Ue=L%VWAo;(Qb*bdf zmGx7Sf48icNd9Ko?vnMVm8Y?|!@n8J`DEMANc@Pbe=h4ivc6x|U9$dUy`Kdw#?M=z zhmYXC^wOwaBJ$q_-pH>;d8Od#1B})GdX!g>aVviT_mvo4J9z!z*<)`vp6f8z zgL7r(#r}PiY;AT|dYbCIm9=hvz~|kH@4Nf~ z#+CuM(eDPeZe{7()n$dn1@ji<2$!sj)p-5v&#c+Ig-x~iJx%Od1`JOCU#&H=>qhBH zH(Sms{qA~vlhy!XC673hRn~Z$>O32RK6hnPwa@3>Qi*T*H{(ABAWIM42)M%#xtlh7 z@SQ$?2LQYBI1O);yTucz3{;C8E@72E_eKN{KT=Ys9;wV^mF^aIO)%hAA~L!JvG@X& z$T5J}#i~__*A{9v!30cpKDb0}WW0l(Ca5z!m6_A%pR>i&GzSRyUNKNVr(xcld2?ir z#>jm^{K9>sJ60tp@#di0x3$FWtMmHs%0rENr59Ql9D4$0%*vq8hd!71yftpWzp$32 z$8gK)F)?bvWUq4Wnyu({V;LOe`>)5NBTlgkEH00qr-qb^JR5w~zO5^(8yfJI#e_l( z7bSX45pt#hj{PLDG|;fJx;cQ6S?NV6m`gw7ydtl6Q?OY*pekqz__pFx5SOg;_yWP| zhP6#d1fscpLZRQ~-Vofl(d{eo;VW1S+P&7bO*HLl3tDR2&HNb`g5(`#rPej>z&XYL zDf$+uhKuGxOwn6dGw@|byuRykZ6H$Rr;%$HduxLYZcWVcTNt~spmcRXQSN-2w}0D; zWm=gT_}`O(YY{3QeTV~(DzF+eHc6g>X%_#`8A!FqzsEFhN__hty7Ok|j0_|S%7Wb% zo-N|>rxGZB0vOmxj^j^+0v?O9a(rKPn}xAA$bc=W_WRw98ydDcS{fUg{7YvBeNBt} zHT7=1Q8fqeOW_SGZ(U$cjkj@8wZC!h=6N&uYn+}sY%15$w^vJN&YPPvb6Hxd!?7gb z3;F~6mi||w<&qsN{n*9%Jb|qO$AM!B|DFt9+_fb>&t`0XHoEtX-^ literal 0 HcmV?d00001 diff --git a/dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m b/dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m new file mode 100644 index 0000000..63a9237 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m @@ -0,0 +1,64 @@ +function G = G_gen_rel(q_rel,scale, torso_mass) +if nargin < 3 + torso_mass = 0; +elseif any(isnan(torso_mass)) || isempty(torso_mass) + torso_mass = 0; +end + +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 diff --git a/dynsys_helper/Rabbit_helper/FROST/Ge_five_link_walker.mexw64 b/dynsys_helper/Rabbit_helper/FROST/Ge_five_link_walker.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..9938291c1b74c16ba698008cbc6328559fd5abe9 GIT binary patch literal 35840 zcmeHQ3w%`7nLm?ck`Ra!Ab}`SFF0Tnfk>zZ2{Ho{xv>+BAt2gdG9)+2l*vpwb0-AL zDshMf#ZnbG^CD86V0W9IA5p4yrH7e^)4DAN+5nrACf9KviOdhnl z>hHI;_wqaE`_A`1-+A0~?+laT+qbbq##j1kHYwC6{=ewefW z#96&e`}Iv{uSR>~{m0K;#o84*Wwv}x5GWG;H+R4~yg8DVU z*rmV+HZYd2(?)^0cQUp;p56i5G{6_Q=Ezc$3|{Rms3>IWJ57eTOJJ;Wj=$O^yBM34 z4MqdXB`B*<;w~Ge%^X#XnUEv44TT`d$6YqY!gCr_L6KtHi9$a8C~=pK?73CB%5h+H zpkR1dBTC$5Be~idcXfa<>pq^)G{`ORGh}K4O&19e<9o0E9eRe2 z*=p?u;jASi$rL&GQ8aqiGTvbobb-X70`vc@36wy={B#W|-sZ5Pg<62>W-DzEy7n;h z@95@pbb(>!i*$k2WWGi>pTo_A2J^>Z9!%BGS<7mjf}nGCt4TwHzFW82Kvpw`T1Duv z+N)aSeLzoCR}tNn%7rVH0{Kl~UBFw*hgypAbpi9xa0YaNN(!f0(sGDaEQhouUO;=M zu3bmkOLc8EMW7+LL1^<7kr|#Mm4X!6bgRijV=C9JHj&joR%Xk zRF$Z6ks_4>`7LpM80Ky52U?0m@-|oZmdpKYQ=>Q6>12!=%w?#a%x7@h;C)gF;ACr@ zU7T~9PCznMgSi?%MK!1n9#3{Yx16u?9acDsDLta+a1%#BSB2?L$#bg;pu;K!apY)z z7VB}S1aNG+k{Sw>RG&ap7Gy+C+pcS?1aNk!p*XDSXq51vOEmE%s<=KX>cm~G`%|Uh zMy0^mPSu!s&suGWZQNXkMn0#LF%KYJsd?GR$*`jmz^T*ys|q>+c~T8-()i!e`7|AP zelh0I-=gvzRyc|&b*oXRb0{`-Q~;ulS9&6syUj^^Z+RDA7ZkZ}lkdOejC&esOPAx@ zaJ-!PrAlqMu!b-`SQ;7Wt5n)n>rS`;*-2|+HN0k||7uj$R;#*7GO}i={O2q=hjf+I zDyiF&)1`;O_wD#F^p2v(@!jGZYP_6Tv(INA!o{S@NdLDA9woBPv~V%?)NseCzK5}J zC*+bDSsPW=_;7!$lH$YVyIL#^v3Y+dGQDrKd$$v}Ucu4oux9f+F~1RqPK%+9SWt47 z*q$}(4&090(;Wbjj}jT{zXZGz(_It6yIJMc5${#-^lV`!Ijwb~GR_ov7-SLVg;979 zXuQ}xH%{+P@GMV;Tf*`UqiC+uxj}(K6;gt!1anOC<-Bdu+9@)H*}H~!p${N(Jv2u# zI9>}Sw=gff2f1RqQ&2^cLEfDTOk8&m!IfE8i>*m%L-bekZa$Zwb~B%Za;f|Ik!kNn zqhe^kIBMQU8w-PGWMFs(usu3(HV&P}Efy!7+(bbx&W zPx2zrj02d~@tZ<)M$dM!FT zF=}X$U1y@veskpC0kDX9xJ612!M@8ORNNA_e7B1?w?b)npQ$ajmZRIPGCdq%f!kNX zsURw6oywWKgOOX#Mx&dL6c9}wpmis}J)6_M*9YGv0=YvG? zMH0BGTpQU2IySZ)0FhFvf+`_ExSgtJ21p|7#0qq)J$-K}w5Q?0uq7P4G7^En=CCY) ze;;18fPYU3_#b32cEEi2>*{+0+q`Jo;mAEOW7{0Hv@DOp=pHhv92Z0ROty+jccd8v zSjrZaDWY;X@;I7K|6y+Ok_qFh9i9Rk!j9 zVk=Sxl=m@3T8;%R3$kqReK*aKpCgy;OQNAc)B39-XVFlOp{5xI_B zj>j>bB{#;%?;>oK9EzHPvoQWwfR}fSR8E2yYrKwUl~{vb8nr}ab10e^yeeiZ43U_N zi7_&6O$$2L+_jA?6G$AK8`yCaZdX%+s_0W2ft?J;4%GAehrCBzpoE4QmvE^jY z+`iP*KY`}q%s)|I4k*79m7~$;wL$XG%#*EuWu*E};wfs#8s>9}{JbP8XRuiW&5^@@ zRPDp`GqEX<&!<4&6gUQ=U`0#k{?tR~L)VcWZOSshDSQUYA|z*=<8A zL$58KbqE4QdEvPEBr07BQHex*SVTe$Fh|=`8Qw_gE}d2O0=s3GBQ>+QeH`}QaJDnC zn-h}7_Hmnv+mj9!FYK1TWLJvDi3^|ekF_iI)Wj}vVUIt#DAbkH|7wxawQq=P-SiQ$ zVje#mxdV3inxuLBB0t07 zbXGFjgzhUEdwvpV@y7!m*DqiXNy)&DKe$f4-}TLfUL=V#FthW}8j&Z`MUL)<5T9=| zf#|kvE5H>jPnutZ!Ip!+qWhG^v2Tm{0NRn#H<5^Zh~tJw8O)53E*fKCZ#9_xZ?^4& zP2sB}1_lmzzE+9~j`XK(%8Af>Cd)S;hW$uCG|`V)SoBw*FISVRucpNY{7%qviMEW> zFd6%6MJ4M$+}JNd-BD$)y3TqR&+6_l6+PeW%-id1E$l^!STZ;qz=5TBz%t(a+}E(h zj^K??KM1-m^EAiDrym8Rp(#O!Y-{%IHdCm_`p)M{&)LU;=yJAG>C%>(`PJjmXmEpA zWzIao@9WQ@jrE+-ynwH~icWRq*|URX&RKgcyX=`_`z_gGVh>Qh@SW}B4vDRq)}n>a z$=S+H{?>lhAh{kPDLvz}U{boafDM=m-gwx{m`x9%x3D@!hJ!X*j}6+&6@d5 zq88$rw=g4`C7`8*#cK$^RK?qi1+|???Lr*$iZjHnUO~(Qo>!P5=5>j?ah$^iOP8aO zr#oA?583DvTh0H7*SR#8Me*GpG~1|&WxD3$4u7oAbi>RLLyMv}V_bvD(X5Jl^#kFM z?HHMRtQJn9Gp>bznmf?$1Fi3-WR)Ven}3Klj{*3PVmRft%DtW(0w1cId3WQf&W;)rK2C-s6X{Y?Twc@yQX3+zrQc6HyKyy)rbzMoLKLK;AKX zXHOFNyC6%1Xx7hKfGOXPaEb382NQD+&*SJXabToC3{_k6A>Q80xm?JJJ2+ljuO29`;Ho!94pQBlOHe(78n#6W+=naQWY%d(JY~M$?NKRWJK;CT8tb2jtu9Wu9 z$tmG!=7Jis{XJ_B`Dorx+4^LYV(7BCU<)Hb-J)L7OP=;6b&(=1141_Wc|CTP2_6r1A_eTC z#%DvFgFx7@W5S3&VBXqZ$7G7mSORa_T!w#9F*WB`1?qc)mxh|MlY)~JQD|@Oec$s& zXfUDC()=E?-Di?<=;u>6@>rs-9OpqhL*UtxpG(E|sgIY?P<{EQc#`X%Bq|F<9Ne+v zJsS}a15OR0eW~KYKluM^*g#o6p&Ua$Vur6Uy_Bu_v0(E|a{$yo1W1W9%{hAZmG_ zwQ#_p9CWl64=UZQYu;*I`C%b$d!=G1y%jE__E2}yjDf{bi=)+i3oI@QC!3g@v#2sT zfyvVrH4wPO-0|LrcqkLrPZ4qEp?}Df<-vV{u~0Wbb22mwaX`7a`Bi2apux3NG90&I z9NhQN)y@>?w65$$Ag2}%ZnWkd=? zKoX()PGLl7HMgQAPYiVn2h5E?v9ejKSq4TkCm*vC&+WEPeiw+Fyq^N)Lx(w=7hYh_ zL`+zNdto^Uxun`s)9@5!EJVs~Svx(jkZRPb{ypHhtC0@(T#J!i- z4ZOaG*G^tv%jUpBWam%3?nT{U+IM7l8U0CZNVkoKkpUwEMh1)w7#T1!U}V6^fROXMjNT98S-tmD8#4?T889+nWWdOPkpUwEMh1)w7#T1!U}V6^fROF|{P(ot%E(@5ZNv95a)*Bb3nsXF-ofU4e2XJ&+1{me4Hx>G z$fXgakpUwEMh1)w7#T1!U}V6^fROX> zZf~Q@mukRmna-xAxbo`|qRc#A03Y8<~$Ud-cv!v9?Mdh_#Qs{_eFO z-8R*x%RgN|f732oL99J5^(U9!UcEThzO1GHmd+mpVr|XeQ@Xwi2ZptWa`oT5PXPJ* zC`lVp@E@J&(s}sx9+rvn5O@!1JmTZ$+&-5}*v2-QMzh?b(lazTLS$5>5mE->FJz!} zg=58G>pu_t`0X#-9=y7y?Mpkp^$Yrc@=LETUm1}6f%2LvS3oXz_^Q;URlaiR63M?# z@<kWBFDoi5ukrf=^6YAzHY>iA}yobv`~Tm1W6eSXOlk%bK6e zvgVENt;FvR(ycl1GW;1Bl;7YNbM(uEUvo*Gmryd^lFTgCsmuZ$%e=7-x=y2ja|X(5 zqv)8DOe~4w$xLOL(9JAMVVTg+oR`r+_Qzb3%r2R4VVC$P^=39qtki8@JPkimgkr`| z_7J3t$5EP4V&ZY6#NyyQ-H#e;?igxM#~;Gc`Ze(i#(sbz!X81clVD;Ah&dLcrmOWg zHwM#_So-`lmhMmHJ-kp~Vs`XcJ}%_;$FTHyn*HH=DG4maH1T|#S!Rse9+qY5WU5A7 zA4bc~XtW8*Y=WH1Cd^M^6XuQO{o*-MQ)ARd1}>NZ{h8y}_hQ~bDnTocpxuGeiE{0D z#{MOPu@`~AjzW?bJ~=WRAa4wMt7Ny&OL&djBk2#9t_GYo{Q=?_myv>wQsN z`nsDd3{8mso0vz}2*+w(pkJ>XdXb)S{q1I;V*>uJApXc(F3xhffZOp`(DH%zOl9m% zlpNp#fN5W1OaNXD_ykG^@NU3!INLdZ+XTk;qHF|y3h=}<*Z`hAoiPteC-AL+2T_tj zuzNXU1t>>=M*yc;87l*x3s{Zv5O5i=73F)tw*hvd{2KUvz!NC@f%gL1XW&o$0WSyK zit-cSI|!mY0sINTk5HavHb{xEsGGo3t6euEtpr4X1pz?LC_P4h=r6!5tcG)8Hlzezx96S{r!sJneVz8TmQxadEV_ zK-&V^z!T@?mS9XG9oWx*d7f5^aa;W}+>7IAe$c|8rN-$F(z=80g)U{|+Bb0}mCaIFMa9yJ zK!a4}u5njY)VaLXcvZsK4A28|b%ng4L8@?jYkU>eQb6|mHsEu*fXvtpuv;IHfUR0u zx^j70QE}ll^K-dNwvAQ!0&IdQ;P&Frs6*Q=uQVTeuDTNU_%Le)B`fXF6+ zs$#W_I_UO7oZ??((%L|=(^VAvp$@VBi+Q`5cB?H%@Bg?eT+yrJ)sS2#X6$nW6Ydsr;rh1PzuxNVF@4Y@yRVU$_VYBhQFMLzXS0m4^^3`AO3e?YO zyk=)44^{ubGoO{f6{3K`4;@!GKIR`j2TXPt}8F0B1YDTi(F!2)GL9 zb3k~Js!ROtMjVN1r9g}s&o1Q0LL5n)QlsP%JX9~5?g|un8-4hW+H@i4wpY;!Y|-=@ zmnR@izoB4m%=S>_nEg135#9^t4vDp3ZmiQ#FPN+6^M(?8iQVBSTE1-cY7O}`ZPtKy e;uyazkM4Z*kw<~_;9WHU+nKSGfN_1XGw{F3`2yAe literal 0 HcmV?d00001 diff --git a/dynsys_helper/Rabbit_helper/FROST/J_LeftToe.mexw64 b/dynsys_helper/Rabbit_helper/FROST/J_LeftToe.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..9a7c7f9c19a6ca23cec0ac9cac10655351e0f7a3 GIT binary patch literal 35328 zcmeHQ4|o*SwZ9v(Nl1vhfkY$tXTilrF%VgbQ9yQKqZ7Kp7!rv#Hd!{4WXonZ-JOMC zwGuW_!kE@t{93K=E44nKU!TunpOt`q&yArapw@_f8boWXlx~FjVx$_a?Y`f+GqY^| zwc6_Uz1kVRd(XM&oO{l>=bn4#&g`-&yWvhYjWL#r5|1<14M>-r*MGel1^bNi_npBW zoBrI~ZiDN&xs^@ckfkLUXbifWEgrYuACN2?L`yL2w|M;)=QZmr&4GGxVRm+=RaNc# z_dP!@*neb1D|4S*Gja*qv+f;u?_v&LeeWU;UwiKY4qt!I0{F*eGe?O36yyp43-%9; zn0Wg&-tVb^vTm

4F<65&`>&QU}9UL6y3(y4oJ8DH)D5@&Oyc=BSX6w zJ4sNxh8a5t`0g!?6>GFf;4t_*lj*ymnC-HAfbkCJrR8H+A#Q3M6@Z5ImJB)v(Oo%DGX zxw;u(>_)-#FmgEQvXfk6gSS4!n1x~%MIpP}P?9b?$rTlO8X6g!LvsBnB-f6TblFKR z7z%no<8v_xgyb++$(NnWiM{{?hlnwZLUP-war&||w(4~2{}=WwT|;-}Vg%y(s4|db zL*F?Y)jMaRsv;~Gb3XI|F_v0Tbzw;inTh+IS*ZSpsv)ACL)`O8_&idzg|K2$@H686 z1sS8rJwU3SAnpjs{edd;98@yV4v}2Xd8l?{&WCK|+HHgxNmVxqM@Zx@s(wt}b`kCS zRJ}vZdkDLPu-nP*x2SmwRX0<$em1HL$>)n^qH>Y?^~Bggj55Lms;(vvJ|YFB)Z9+O z&o4yvY8I-h3{;PimIcJ!W=54q*poDbvmnd^t5=_O1cc5?t6AuDSu=#Tlhs(n-Ixg` zY>XNO`3`Fn2qPwQsv%bOmw0@{l;gD8HG!0|0uTLJ6%eKnn5Ahb*969yXwWoNYXWIw zO}wB96u>8^wS2^MJ^X=J?bcmz8bo7u13{1lURQJGLkd%qs&2O`6pW^rIt_?wC+JeT zLL=>s#C$?yD&`c8-I_*43K|s(G!{+a{PvI4_*h7*sA!Twkh!f6dr;e15DzZ*1OhIrUNsw?Z*hGSFXoAV19ONR1Z*mA@3bEa) zaA8uRV8AWVE5l8V2>nk|J3TM7kAMirLv zecU0SMk9b@R@7_8(Md4~L}9_x)+A415WNaCr!WXOrFdGT5fx7r$scH*Dw2w)`I=FM z3tI{Wn(4ktA$Dn=DgSyI308hgC+nNu;cree&Pijh4PNw3s8^HB<@I=5i(F5`9XD4HscUqOTR`Y_X;1I;}?pDN< zW_6F2C^Jv{k>vI)jjFj#euWebnqOE&=Ilk9aqJV)ZWfi;qWjVrV|yXTV#h(= zn*mJNco2iCv{nn9shMN?SMy;$s`f7?p&}Z7ajfTqcwC6=7bXUM!Yt^7f7J+Jbvkjj zjGV;wgkwTctP5v(WKDsZH52%uvHYa-sSHI2*f;ScEC4khK-wm+`1t%g@5Vcq`P+Cc z@w$Q6)x2KA>tj2~{KR72$5nYSp~T-<^Tbj@&$R0AU_lHqv?`=wU7Fj z%D;}k7&jI}of2HRhBgn$if!a(tAGGn9AEovw~s{J^Vd_v?K|F;josg1%yzURdTXjJ zdZIku*J*qZl-O5jTsuY)_2S5iAA*vih5XH74D>Qoo#(cbv@9MOXXqtBcw>j;jroQJO zbMY6|N%B$VN!GtHQ$t7p6pv$3CvwTmbBXM{D#-6*vj`hwvZ^1df1OByVx9s+bMUA; zZc`9sqXDn&1o;KIPp~~5d2^-kVxO8TF4_3IX5#fY;OgfjBml?x6*pwt#k$|nfO zL?sgKVX^t}0qSU5s?0vU54k*!qvi@LNX{4v%#G9pZPwyDhiO~oh2gB1AM|-==ZeXn8azpq% z0oH_RJ#EGG3OIahZx&X5!{tK0X=k$Uq(Btp)cHji9tObu|NS50ah!nWCK!S&WSlSx za4y#t9TrTFX3s#-!tX3r_Tb%y6CWfzQ4p@@M~a^G*n=mrZ=W5@MjJbm9dEgyK6P>Q zM0)IUG(tpl5~9ldBxE4EZSTz>}Vgh|oNSiRCnk9|PKPEMke_pyK*dkNK8 zfywf?RQ`|_K5}b__rL-@EAl5p7^!L8Ym)N#6@Es;>8pIQgdQrztF&?PNp5n# zLePhxm~ltw_95vgc^08xZ2t*jAhT^~O^6fqMT}OWH}7&B(wc7jI&dwf$Bo~I!j6Ma z(0$b8?A>m>9qrgmNXAA{;f6;M%#BfBG^RseKF||y@92e2%U35%3=HsiRS7bV>?iH= zk;sPz)9nvJf9yZe6XRHbMgJv?<>W@tzM7es@S8vzCfe1UhGaa^D9CxYd>#8mq%ST% zt*o=bl?(cI8%kg7bJ?DDb(RdG#7t%mhjCyj8#d(_zY8@Jc{$mgpldQ69M8$_0;H)a z$AIiN_U<*3t9QM9I#ycDJ_*E-^IdX}y3~w6J`#_Iw+J5NOF(en{w@bI%x#zRl~>!X ztUO14xYD)YY11A@uF!RDzA&vHC|~%lt{I1f&RlEhvKOR$dCd&hvYue3yb4R-wJaLM zM)7pPi8pMA1>Al3L`810vReqt#KKDhOyTP=f3kU+Y&=R{>^KsO57B)Svz66;G@LHn zp0^a!FmcKi`K>Fi0hjGKBBiz;4xfPo$oJ44;ABi)Oye^}>4(|Y~2sTMu-cc{M zQQBn>36YQDVLbCc6Dz}`&Ivq#zlh{`0{mDJ2)2VAQNiSV2D|G-mlDH*b_cX$I=J?` z$7;_gmHUSNh~pZa61%cDgFEyRnu+clp*0j+fKdn^49tmsFc9Zkw-QatbBmhi79k?$ zGvSHF_rFni&jK2f@t2JN%X-5Bp{p!Eo_G9qqL$#9w!|FI8$?Tv3YQZ8nu2$gS(Nrm z)Gom>ugom;3|a&ma9fF4u=NOgah$^iO&6eH(~K?aMKpSZPUE$WWTXb?v=92xTtiJP z(;GkO^e4tlH%zk-S#fJQ<~5ua&pRJZojz?Rf`rI5(+tMvzmHFb6f$VngEq7#O`!-~ zMmO5r2jF4?r`Vc*PrFk_YV3{U97%z{N!vZ-%QX4O{4WDl_0U@nVqD_x0Wgts zc;3c%i326eg-E@%7~)-noXdq=xP#-hM3D<8rmn6SG0-d^orG9y2jh7!caXA(^_Nb#tzR+dtBB!Q%Mi}p*|CGluk3W{%By; z=!qv6%n0A-E=!zcM#i3H-b$Qh!UK_RbYf?0dOp%U3WSY2CQKLz@|GP#GR5bopre}0 zurJ7lwqj4Hc`$rVq%}V^e5Nc|y4nUm@*R(irff2`eaP(h7$h9}dFsag3$JM=>P|dc z@^h)sb?$lB(o}u<{0i)EXA1H%0S9;Nc+bZM-@*!iMTqof2+MvG{N1>N>VzmwIP*gH zZ=n>r-@@bLSl#WPT6cW@>-S?kwAY#Gu{dAb@FHRi`>-u~z_@>d^5|*0-zoPTFg}VV zrEA-}9!HteDIapmhaGM2ihjxKXdChR#ZpJxuwN2=C0YGW`KVJidg>W-w7usDOHCzN zN8}fsSx4GNt`_}a+d+BMnRS@&JEr?PONO2DL1$;#sNC0iZ z(A|KZuXq?M!2#v$wl|q+m?qa$$8g++a&Y&8+ntU+omGQy-?m+JT$P=wu=%quYC}Qji|8f5SHk~ z1dq>O)kbfLT)e)L*AMafUS2QRM%=4-UBv5Eyq>}9_qog{uV3Z$$GrX(uQRyJA&%E< zB|U3-ZRho3UR!y6Ca)EG3dc{~O7h2fJ;3Wjynd3`kMMdI>fMIkXUCVxpVa=Y5bK@v z2eBcMk>kANNlJpy_J^a$t?&?BHnK#zbPfhi-<&|o^hozWE^oKi?<=n>E(phrND zfF1!o0(u1W2eBcMk>kANNlJpx~-2-vIeT_cMizrCYO`L@K+pP$mc1DfE)TB7(~ zV;TR>_>DuaB?O**mW$v&O1zD43(5GV489JA|E6tmb!9uAqi+M3w8&NX?`LIov29Rs zC%%tSH2w=%P{F&~4z?BJTO3i-&K|jEywDda=K5gt2eBcMk>kANNlJpy_J^a$t? z&?BHnK#zbPfpHPw-&N2nJpy_J^a$t?&?BHnK#zbPfq(u8T;mt<)9@`}sUUQbB`AI) z>%%KS*b}X?V&_lp${k8s{B)$yviTf9?R8!vpxl6{74*o@|!4bMMmSzKodW0 z%_?t3*`V;K#?Q%pE|=v_w$(71=6;24R^bGZNjpuD(g~A9pkkeK-BTm4zjnuL<5zyN ze_=&``um;qzv@?CQBxHXgQ1!Rk2@sQI0GJKNz_!WUL^)Miav2mO@$Z|-9b-N&DEuq zH4VW)NGhxsHx)LcgBYw4TiwkqKCvd~-mt+dSqd$l(Bk6DY8pj<%|`r)L9<)(dO|g8 zYF%Q3R2dK#HjAyBmoC9?`X@3Czum#8&^1c|daXx5kANNlJp!L&1Pqx5mbnT)LZiW) z6qaLHzzk;@*jd{Y!#Q~wEKi!w^6JxB-jXzyw>W2T*COSIYgV6!KZ}BrM!$WfUIzSn zOxog<@*GndGu3A>6Z)7IXSHa39zUP4mr&+S(#Mc$V5#I!ZU)Om-`vV{mW%$mi_I;h ze){Y*HhYPQ%?_S9nAodoIPepIBC8@M0o2|qYbv!nQ0G@aV> z7}NBvyO6QpplpXeg1k=({#Fm#2_MzI>UfJ1FgulHFUe%tK_x0vjU}N+^W~F5u75hq zUaaaL-!DCdr5k3Q_A}3jdE3YG4Bd%&O4yz(Hj~k2rm>k)2AjDgoy}aFg(xVOejL*x zp~I;npgm*wZd3M*6bMqOM!OEB31w*xV?Q)Ab_?)M6p|c&USv2nUKRFxByYe^c!Sp` zYL9^K7Vjn;VTq$(pJN2V`1LvbM_|0zoVXeV7{JA#3Q}`=bG2rHN8M_!|0C*AL8kBzE4*;G*soTa_2F?jK-1kp` zz7;UV!q_FCTLI6W3mWihz=u(qfIka3eIEX%8t^8-o2f(?E4Y9$H%cn-7QhEkDuF)+ zco4+}ydQ8B#Rojq3jHV&a0_4?$`auB0X~aT1AGARBuYK-Q-CYx<9!J5a=^1Mgpa^o zfWJc-fIl<7gmI$G0DU=NBg#3zCBR;ke#o3tfbpXA0WStThSClh^F^Se>;&El_&t>S zfIkNK;l+T!UHDzp?I;fbzYlO2!P zxloNOQ{h!A>{IO?Q{jUu%vbq!YWpS?E?4P3wS9*QKd<e(bc|D$Gf2H)D0bI8Cd-yj4Gn z`$rNj2wD`hjHJG!wC>P%s!QJori#ozM%VX&PZz&`wS8cxvAOkXQ7U8XXTVBBs{{1G zODo&WTh2gugHL4aCyG>gkln3dYjKhVwmK-fB~g(ob4x*QYwcyWBAk-F8=6}e*86;{ zh#ss00UO z#b>`aC0L?=lNX=f(x<)j=eoHaRU+UQTfI`PyoC~OWDZua^Mp#h($ zNlk^mOA9Y8RHHMFkH3vBd7H&#k)qUV!(wntxfpB+1o7dvM_e6%6sEgGxqaD=YH>lMP-Sq7&!M4?fHUEU2rcW}#U zx6g;q)2EeUx|A@kb0KB|)UiiWDP~gXKYd_=rin%UI=C60W|a)g!jhdmuPT17#J) zb)qyS`55eWpZ!m5C$nw@5nE(_F$eWJkgdp%=cDXCan;<7DP zGWOH$QM$AqfiFq~mSP}RtAkksco|BTT2!(QzQ7U4G>`q>&Lufx+h5?J>qAS5Kq|)# zID;`NTQ!zOPXPf3%L(-95D>tt9%SqW1Kw9E9oTYrC?qy-@NKcQHv9ac74yPD{}mxm z6TUkXDs1+8@QtHDgH-4VG+*HkH80$B={))xpSJ;r%&t`QO*j%YilGEEnO(wHJy$%nZz*fv_aQi~yyepS4O6VTjIiWvEVuJPMi^ll6d{JW1 z=)QcB7SAio9pw(Evvlp%)zvEUkF;3}-X&xHgrC;^wCSfn`tc4NfbBBxBA{Pi>hwB%o+XVY3Xd&9X~(XA@#= z4O!5Tk=EMyTCMMEv_7Ao&u3$wH6;4gC8Q0BS~2=5F%EndIHdCeM2OQ1nql%1VvRaN`` zW7mBJ`wow4W!_V(MlVEr&OL+gUBKb1@14)#Yww-Q;p^{N0ROmj_9*e6hFl?F!M?#! z6K}u9`#pWnVczfQdya5e>Gd>|4Q)gfrHnP)I+N|X;N`U%ZG_FYWTl+KSS2Wn6>4b? zpqb;F4Fm{JVJwZ4#vx-hWo0BA03GlnhCig|YfY!3MYF zX6(t+z-UFuN4X9q>9Qkj7Aa!PfEckmPzd@_k}f-A(M7F_pg_LuKp~r?H|esIKCdEI zKMRbVD3~5b4kul9l51-8HiQ_nP|TtzWOoZn(q$*Pq9RXY6Jv8pZUBYkI#7}>JIMt@ zK@VtrE{1@R9Of$dvU54n7l7a}F-A~GZYwp$FFRu^##{ftuxIHSzB3mi5YIuCfg~IL z&grQBa4M>5!g4X^!ygc1i3L?Rmc+1`xZjzB>gQAq6YWgmo%2D`7@b)kDG&61kJA`>5LvqJ5vL zKTz{-!Y(B2HnRIIYJQWdo2k0~3{>Zl&*#rZl)5{NMUMHH62!kg3**vr$JHe1YJs3 zXr#T7m``d<#hjwCN7JZCL8C%}#-furzx87^J{Hm{YFaTA7>laL7I8AvDFkr(RnCb9 z?xRKkX8;`2_0dri#0lrKQ%}mr6j`SgrXV<&BuF?HY$m}sG{NLh4sj90H#vkch1g+L zxG+p+h)fkZ#?aSj34i1!9Iq#E27;cWAa0NyXn0&6dK2ErkLlV+zao zKJE}uqY=O{E9y1l=%g3~qOjm;Tau?Rh+YMnQy2uCQamlvh>E9*HMys9ZJKgR~?r zpQt1!(ly>>Co(h!RBACx>NhiqJy~a_#+$4&OJhvbNlxGA=1J$YCNkKZy;h;QXsO+B zI}bS)WG}?C*GHBqiSxGUo$8Ag?u$< z|DvAz>Y^2S1NTmG>i1LWw_DZR&fB3dC%bZrW)xXCo*E{Ns=1Xr%I(pGTbsq%<+SGWI~~8#MUD%Ro3R?CJfS=9nXlq@*PVS7AoltU z#*Uo)ZWfi=qI=S@G3Jo)uBiumMx8Eg+yt}I~tnTbBYoKf%9>6-I;qzk;K>%tj;NMWm`75pk|4(Md z_8Vmw!-8k_;`p{_Vk@A=-Vrx-u8u?LO{7#F6e1-|vI=s4%mD&4Wk@IwiPrEo~l>72C+oRsjLDIKKMXZXb!b=U-0|w{Lq_Huij-G24;O z=q;(X=&_1;f0yw=P-0)Eacvtz)Jr1Ee+Wv77V=*#0kj>m zZ~GO;I*0eh3-YUC$H5UM7#oA0b6L4~0#a zvMlh-LSRcx245@q*YW8uB1+IYr3Aj&8INZuu3&I(9A|kTjOGAwk{+w-s_E zZU`^L{C^2N+ac=mL+}!fxA2UTXl#S#&bVZZ#Ak#rNa%_}Bw=Dsf=uuH6!Nh&+Y#h` zkrJ3T#Ki9SYTZ9P%j&%PXgx_*WpV}itil?~vN3s7kcWlNBVl9rO2aXnC!UGD{RbM$ ze)(lVelGr^I!QjtJjwbuW@`AzpW<;W>O?M?c`lKiR|WZ9Y!+c-Ojh+n^{)~sP{LDS zcrG4wCv6IXY&77toglv;_Y1bYkvFdtUhG$M#U&em+d{lvhdi*`j!;H^bLGN=5Gb=n zlky1yGEs>{dsu7%e1JOIma4MH_aT?ZvCLuG<;=(}@1BLdH=6I7(a#BKLieoA<=v?V z%9r*_XFKGwS;Ep6f>{pPkujrJSUM0)D~t3N9DB1&?(LgkIx!ld^FVk;akQ_e{6@wa zFEfPS6JSl4(c4}^uYkixcV}VcH(n;>o3)o2UVQjrO$I@e$p|R6Iu}X^QxDIT}cJ!h(t=LlSaQzva6DCEoV+~3pJ@x?^J3ft0-p2xN z>?Kqa1*Xg6GWkPV_{gnc-UAEtw8)~0X5_-4{uhJ&P zC%MV}3PB%&V#Xbz$A_e&GPhsG zS6*F@vhp1H;VRd{y{27`T%r5gd|}1_P`>b8-Lnn~UAfk>r7uYN@~T;`rM8!10*6m?me6(hsxyqh^Z!VQiAN zydz$0qqNH&6e1tR!+79-CRUC|onv@#eG$p=B>1r+5NroJqk_r#40hM4E+vKq?G9+i zba3r=P1K%ICif5j5yv$;C3a_T1b6r)G!xx9LTf0t5Tg)27@QmZU@*?NZY7$O=N2{3 zEkZ=hXTp=k_rFni_d*(y@fS@1OZ&nBp}Rakp7-``L@mWLZK*k)H-wfP6)qk;VCxlj<2Z*4n$AVTrWsq>hiLQ)UB+vh$Ve^DX&(%r zxr&-trfWax^e4tlH%zk-S$<0e<~5ua&pQWBji0s?K|T`p_Ask9gVP@gF|N~alG ze>AWf^u&`3W`yr^mnP0KqZ7|EZzaw$;lW4`Inc@pl z&{550*cW6&dxP|dc@^h)seb(96(o|h?P9^rYQw4dcfP*`Byys&>Z()VMB1HN!gr&a@{&vzqbxM?` zoOz-9H&6=QZ{hKAqVA4QtvkNp_4_d%+Uv~pSe&nIco8v%eb^S=Z``*|dGs{h@05G@ z8y`cH(zX3vkE7h_ln*-PLyq=$MZe^Aw2ylIVwt0T#4m}y(yRfee8edmJq?UG+TU}8 zrRLJC!}5#Hti$c2SBw6z?SMSy%sRyP9n<|?r6W%HfU~Q7Oz!Vmd!noQ<5JxADuqaP z8BE3v&MspPG%k;(8JJYCygn_3N%NMs5;()S^TUtvP-fXUSHPKv{vlGP zANPf_(A|KZuXq?M#R28?_BWYngeKQi&v4v^a&UKp+mntyUDZQyyqO#F?>BCDE3w*d{5CM`Q=Bz7w1y^EKm1WCr;FO6BcjAdv^b0Yng{l+Cgv9ej0 zaWOE;oDyUup4;u8{w^>jh5s%BpE~5MEqa;prFy;wc?R#LoRYKKcTl3{bqwLK5fzpm z#1fsF;PC}3+UYHki`Q51`XOH5!|TOchUi+H_)*Ry#2K9?Ee^{c%8nAgALbq1F? z$no0Eq-Qm+?Yu7LwUyVW@>-#%aQwtAB>y(A2YG#v*H7{KQC{yrz0=V5?Bp{2liJ@E zV!e|d0X+hG1oQ~#5zr%`M?jB&9sxZ9dIaE(aMB1gHk!`qV06WYPAa4`^a$t? z&?BHnK#zbP0X+hG1oQ~#5zr%`M?jB&9)T}Z1nkxLu8~EM-`Q5Kd|P7p&rfUL0Zs5? ztxyy;ts?EcAtnxjq;@0(u1W2eBcMk>kANNl zJpy_J^a$t?&?BHnU{VD5cNO$XkANNlJpy_J^a$t?&?BHn;GaJN*Z4*JG<<7VDhQo# z35s72dxK(wrC@_Qc=7r8wPjZBsB$@0TwcDW+1)BOT<-O6aQnOstHUj2)G5TO0s%{l z+rP;Y2umPX+`*=B3;w%xh&oU0-{NlbHPf$^kpT;StbSuK;BSKL6gyD1Iv`bwZr^pH zM+yYFw!dB7b2nXwuhb-!_Hd%D&_@z&RsLyBUgb|}PvmO9S)T&(zo9(!4HUN`qw!{; ziJ!J+RX3xoQ+QP4=j1+@%W?H`aEi#Zou)|XglQsBxyHHXokI`)d7t(2 zRqemCR=of4B`?zds$Y3|ZFNWthH4u`QHUH-8!#iDYSS(#U+>4Hi`b)_4pBk7PsW} zglbpSReGD6rK*6qs6}ksxMVSY)L%qGC;Z?%&5q)0 z(M)R3XH3(#<~+uJjj|Q`2=YED29|<$!bi2QI^LoL%uZ$5i!)hvP>IUP#*)yZ`SM92 z*FTeG7pwXw_e)P<>4rJue&!i5Z+lptp(im<3ER`fW;)vJG&WnxV6zvev)RR2h=OwI z$1yDuIy_kfv}X+8t;(K}0zoP@XxF1Oqb$i`?1yH?z6E?63Q0~mFESh(uL}D;k~iQd zywU3uwP!$gt9Jv=u*A`?&o2UD{Q4YzaxG5lp62i(%S2n+Z#q7=)^e!vCHnQniI=`# zOpM1rTWjZR{AM@)u+~sJ{#+a23H%kT7;y7hj9q{-2)qbz70Lkc2LMl?)Nf%d1Lp)A z?)xV|-wc>yVeCTCt$?S`0}Xf$;3Fu_z@G)2IUj#h4R|x)%~T?c6`aeM8zmKZE8qht zRlpwyJb>Z?J^(m|;sc&)g?ouD z!AIaOz~7<_!k<}R!Z=Z8fxZl|3FS=S5?~+70A$WAz<5#mftLUtMd^Ty`Fzk(wgYbi z{2t1^z#j+v@B%>KF8nU)R+I;T-wQZ`@(A!VFT_|;`hhn9K8i8`JPMdu2phnyfbZZP z)fjMQN1Rd29m*N)EZn)R@Q2P`SD@4Yr!&(xP#ytJ=barW9|PAfJpz*>aCDDC=~v+s zD*S;8Z&qQm3aeCjxeCoHJh5BR^QsE>sqj%1Zdc)Xs{CX>KKFKyVxdHZ`6~Rp?B1yA zulk(roTtW>sqiWl_N#V}s_;P-=BxaAwSAKcm#K80+P+mAxPney4T_o(`J zs_;P-{y>G>RJd7%pRM(&+D4ujr@aiDv7h4pn?ySS+E&m;9v_!mfjNooWUT&YkfTScIo@T$s+TQ(e-`c)5Y&!Z6DZfY-zh%l*$?V39z!z z$^d=v(#Cf3mNO7u=Mx$Gu_9FwWOpgpYMf+&tqh88NmQiD-BQroR(Gkb2&ZK4x|X&@ z4L%<$qUY+;V6Z&YRMz0|H&o%TJrh?{*erw9)vc@xwTd2Zqt{c{?DjX{8Jw{NpogS} zI%!j@Sm*UO2I?BbkQ5ATs`Gh663&{?+Y%CiUAMBbdUaJkZ3+j)I=?#@3~a2!=eZkj zx>kC6@!9W936|*J;Kir6^l2~sxo&Ppl?eF7Hm_7Cxq0-KvAUqx1h-*FKQ zXuv0GQgfm2;=+py)#yy(<8Px&-WD-gq$u^;uo&D_AqE=*L43IF5myEvg=sOTAXip~ zgF%eBA{g+9p-@=^%T1zIH6tY&KxD5Ibx}^#R9HFc5c7-r@n!)J}QD_ulmv>#z z9o)3i?epRD^ciKCE+vd>T!@(fb?nj9O3Am<-6~;bRtDe-vgl_-a|HtH!>!sooKn9O z+=S1fiF2JdD23g=YCi%2Z@xC8EaVi|g`1khU_}rgK4Q}DG*eWJkg`+CN{Tw1xh z)MYDHGWOH$QM$AqfiFq~mS7-PtAkkucqvMjT2!(QzQ7U4G*A5A&V@M>+h5?J>qAS5 zKq|)#ID;`Nn>Ch3PXPf3%PI7k5D>tt8Di{41Kw9E9oRB=C?vM5^KG)UwfOv@<@3Wq z|K%Z1Grl_%Ds1t3@QtHDqg3b#v|R2EwJh3j@qGFkpSKZ*%(E3z{! zmSs{f9Fpip{|U9(lz!+L!r3n9l{Rrb7F^5d114Az8!Cd{4LA}tiJ=5DnO(|{g*cM9 z#0{d);-h-`e0QkKzag+*49>TNy$%nZz?RQ%bo)Z${417Sl+Zn~b3%WT#1!kxE}G!$ zvWpUfM)zeGY4Nnzpw5M$@Y0@PpP}EIo z+G#oVUgpI+KR4!_^SaH|&1pe@GZpTofFBjz-A~tDsmNce(%PJ?Et92fH}+ z_q%iF^!L8c^FGh>K40%g-kh8!CD(6d;}~PfD6tr0oq%*XdHuJmL9oyI($2HkljC2U z*=cgWIJ3Oo8?rS919d@9gRR=*_Xi}~I?)yk`)yvo&2{xkTSK5moSTs`!LF(H{ov{E zsN@JD{5e_%zpCNo0*(ka#&suCCTw6JpFpIg6r@-8)bcE+@(5dyPRB3^aUW;N3;PHlKWR`3|&sfmJYT4e_=1s)py%Dh+q}gWZ$3-)vf2D zdSg1OBEpg}@%sKa6V>^1P7Um#`dC@Ll4*I|CJ$ z+eWH>MBKMX?zdDivU@Ypc9Pr^vr%ow+UuK5u5BRf17bW%!Xd(Lqv~7KZ7b1kr|J!A zwh?wNVH?S=pPCI+`KVg{B~(+%=X1_RwV2c|C&o?0SVC9@X0LJl5f|!>`q<_@UF$)3+IU`=*&{itl&&VW$IEDEq0ZHs0z>tvFZAt zi8dh8^jA?c0|PLoY8uovsZ!9SQlP2+G|op(Y2oK^Q#P&*W?^cpX6oN{Q;B}iiy`lk zXg^IQw%AoJ?5h;m&m*Ucbxkk>16EbsJeiXrtrEb=);I@ic$7K;9NPUY*G7+9AWk?} zv{{c^MUW3Q0;i^?OI72tBS`4TNzg$!C(IrFtjTEp$?K15sI!xkvZH#s>nvfVf>JIyn&;2eCrcrq2#_0<6_d zs9b1PDHwpY2(HDxnX5t^s^%tL3Al+tZ%(A^LD%cbw{s;>u2SGkqslTD;B}H~QVHPP zp?6YEp_8f`h{}S@qgKNXmseW-CSB;s>nd&%1wVnfGF}qb}!XT*> zSlp^9y+iG%6TsQ7`>Wc8o2m^UDhsJ>9JZFMNqpiYo48+%d~hCx7F~$%H;F>}D8pJu zf*jv=P9xW;dqs-*+^3!#QCXx*%f)LREm3L~)q(C-6_{_=nMiJYHuq>f9~+B#P|xCa zjvx;z1s0>4(xqxYod8ac%4xBycF{?-K?r9di%oih_;#8YICwmZ2O+8pjgdul+Q3?G zg4`HcjLRvySEQIHk9u-MWkFXbqSS3N+UM!G$2Xiw$tIm)Ivs<1IaUwu)>sT4{8$Y0 z>1?Fv48EY%QBUwKRcDWMI!$NreiSE?JQUOTVeCIu=ny7c^zOaU-9_ZoU_|`R=t%Uy83-a`r(c>#!d1(!r(;Yy9b@w87*kHi$Q;QKns*OkE$$lQ zFzLoA(B_IC3i+8Za;ze9i@h;P$`abM?!N&i!}g440h9;LjP*|gZ>{MYQ^4D!@_LEa z44yv9SlVuTlOU&?lq*3Npx!nHZ>h%18G|G zk|*f>x$b&CQjcr>oo*Qi7?LcY{5X}pCt@)nvb$oe=`+TRGDt=S{&_NhUEhVb(UD^~ zGULr;p3*)UOD(cHM_VnU_@Uwagty{Yc4>Rq&A9U?0JZMHnoZoZV{^7|#XaUZck#M~ z*Bf|U&uiR>2KW7cBlE{{S`SDIBk!1`9IB>9J~l}c=q#(crriFgSghZy+zfzCmb+Ul z_Ymxx4WW|OsO9c1-lTi0V~wWJR*A6NB)X4)4Y+VMjM@bGxLbbDu~(V-ZY!1-V}x6G&MoiB=58Njjw7wnTaz8p z{?b@?oB1hFlp83nErZBp{>H^ZgXb0m12GfX{)d06U13856B;5inJaGTNY+nVf$8^ zBR@kf+n2^7gQkraD)bS9d=N*5Jj}wDcC?Ug`E}%4nT>lvI!oFRC+~sVDmfA}g)hSR ze;GW-0qXK2@ZycPaXTj7pqtjMG07Z>jSF8G*A<0G+{BbP8K*+css0ql5zPHOH8D-d ziL>y=s!u)2+PL~?JxO+Ta0Pir>q?qsgYt1f?h{&%gw5^CO#L_!?pIFsQ7n7pUkmb! zu~)T0^3lwb?El9|^&R<3EQUoLpG#Igm&ndxL4FULMcAy|ujz;Cm*P{PfKP$G>A3AT zVp9-g^CU0Unf!{}EjXT!ym^K2YPU95+_L$Ddg65z$~`-rNM+=$D=yjxfnrBAVLm}X zCMuC=4^zGZAE1u5rSgoSeaKy1xVX^rv@0#Mq&*#bZ#3IIuA385g!c5!CGE+3OBQuY z=M~Du>B6E{f)fkn!nAQ+!lIsFN^ztsr~l1jxog)j)A4A8*1h3z`O#gSB{+uFE;NNt z2(TuM>uN5bkG#T1cTB{}uU#l)Tec09Xl&<>MR{=LiZKpQ-cxtNo;bz zBG8AVSn-O`jnAq0fh24=PctxAS+130jh3|J({#DYRdcEsW~{$``)7J$;|hmT50u^oo=%UzP4&)D@f{FU8V# zFNy}SQ9Pg1|2xM40k1xMpdvTf+1Cl1fQ6R=n8ep%_E_UIRy^vw*m6j*AOVg87_Esd zN5ZMX?O9i17)B4dD!+M26}W86At|}#K=>>iK)!zzr=$JKskgPoG;*e>{V=*et)}cB z!Y1j+>aW2zO1tbnA@Xr7j5|pCl@h$q>&LB!S1~#EgRkU);Mm(56)dj(*j-1v6b}p9 zuYq<92iN|C;o8%RGHdXjeEOw3lSZvQFMc)FOOPRb-824WK1Qh53XZ zQ}Ol^o7z4~?IIlWN~}UxuT5|OcNAF#N0+bz$2nZkG#d?vZfwymWTQ)HGkeyNkxHD? zKI}pBdTL^st~t};kH<_em{uXO565xJt?ez^ozjw(KflS5B>0=K-9vtjE+3gw1XR;QXZ2M=`;YBT zyzt{AzbKZid}s$gWo62(FibBZ^8Obuh|IB4CnR&gaiFlXCmH;wAxnf z&7`cgo0#pi`?s}{8LZWIPAu%ahpa}Tc+1gx2T;XEZfQMmR$RLWAJyPPWG7%b?a#5S zEt|1G22DacICN{rDYO?2ShnpVTp**@L4d3|V_B<#;#L19spbQ7+9X}wacWxy88`q{0FopjkNO`X``ndNERrnMAx!5``~ zYL3!rhSnbiR*#-|=YkR8``o4Rv&`|~XPLL-XPNN9NM{Swp0(lSNar9BHsY8tDiF+D z>+dj`Vsj?Lnl_hVUyx1B1=XR3-tf7RrtIYKR9UdKH}`(*`$J?fX@jNtBj&u^poaeC(j3nR{@Li>zwxM`@qa%&0px2b}>NWj4zJKoF6z#CZMuM3e~X~Lp6g1;Ye zP#u+}QD1}bgzTrhg1^cigy2niSmy;;J z54q&7J?1VnX}UJQS6x`*a>@H#@`1wU_e8(sEo?sS^^3)Y%>#Z(^c79)amhzqvN=@6 zSYh*t!mw0dH1Ux9s%zq*=Htsnf7r2C9&}AS!1o=?Lv2L^E_tu3tz=N{Zd>zy+p1GV zc-boxA{p1gWUMgKojhmYikQXKW}XR+OQIFf`DSLwzsLM=rJAcf=I;Q*KE+wnLn}$* z+>jf}Og%!^TQ*^0kI=RXx7LfS_LRjb;e6TLh#rydv_0l3AedzAWRYTPGhc_6G$qn) z+he{GC{{LWGrNG%%qhUE#QS#VCw~=4n7p3?xeEiAH~gTtU*45pQkw`XMF2^ zny6XZj^MBn6&CH&*~2Wx=9Jw+_kk*Ty@J=Bync+=uCEh!HLsn#F5~sNyiVbD8n55s z^64D^HOI3#ewgFFn@P`&ye{SSQeH3M_4x_($sAAS^+`N=7Mr8+`Y^9w;Po$g-GREp zwCjbDW$brq!@@QiMh1)w7#T1!U}V6^fROXXXiWv%#pRW?!9YmLtr0imHlTwTtQ4C(4UImrGU!>i z&MVn+ZPlUtf=eswM1SRad`UoqNAgyODr>H)D)ZLWOXUG^ZiCpg@sb7jl@NTF1zq@V z3|LJZs_6=5I@`p~en>SvDJzX-N#j{o zO)AS;kixR^C-rV!pnfroV>TXcK{;7IY zXz^*5ECM(+k3Vu@p1Mh1)w@C=0U zZG7}Cllr=wXB(av{W~sSTw^;>i!U@C&Q;gzNSe|9$H_p)WX67SE@OGO;hY9|>^%Iw zC~$_e&NP%9;5NX^P;9`r0Ukx^06qvf8|O3^@J7JnD7OI5#QAFc`S^KF-~!+SD4oEc z1^fslIl@@Qml^Y+yZ}5qhp}r=%7L#1+=lWn@P`3+pgaIP3aFqw3;cb+@fYBX2;2(z zHIx?McLF|(@?+rLfWJX`68KTT+zau0ufUyv<8fY-fG-C86-o~9Q-HRM8LI?-G2pc* zKHxQgKS!|vPtQfXDA~Ys0AHg5oZ-IVY?PzG>j5Ju$ACW!*f$UHw!l^%&Wb4SgMKGq zFUl$4>GP34lx)b91AYg^2D}6CW0VEJGxHfcjGvwqfDgEkXOy+T=}eu0^QVN|(ph;f z%A>&PjJqD?6mUA9Zbfm!mT?&w7@2{iJ5@@z2A|U4JsP}4gY_CL*WhIuv}*AE9jczg z8r-eH$2GW3gXe4VBmMZ?+nuU~0u5$s@bj{Jy{5nXbGCE7mRF|1D>c}y**&VkM>LqN z@z-kYn>4ssqx-b>of`bS@-tFz@6#hGpKW`mroTgjk7)274Q|okW(|I})<cGj;=Uvy(G`Gg*V|jENq$n6H3Dp(X6#8q*niahSw+5Pv#RQVzt&q94vJNNPcRtRScT8=H{dJg)t+8_=6_S1CHgmb@u@w1%8zsM zFo}R)Z1PG~l80w+F{=uSb#NPYG)}F4lV}#J5}U;8uq5g%wbOa-RSk_LIXYlmFjbS^K<9tYS|gV4~Fqw=?!9{ zNM7*pfGeLZiW%Y&NjKcAxe!Z?Dnn;dV-sld3-)RM=`D#!=)yAr5iaDppHGB zTqgOJc^V~*%(4Jn!7Ta((cFQ+`f#Iu%dN;S1vlY67;#p6gHqVzTjfV0;LX2`D-OBD zb>X@?F<2VJhs7AQyUeTn>V&H)YN{3+>8>4|q(E7jd8H_wmi=EmHpv4Mjm4Op*Rn?F z*D?|aUWru!Pjx#DBwZ4y3HwBW&+ls)`)X0yiXyioU!Ad^Y>zUejSPHIGH?k3S*`_h z72u^P6Sbm|4e$ldzy#~?=d3QCG`#%<4&4YXAp^-AH{lG1dy$N7)>%3|2?QK0N72Va zKmf0zm$B>dA;M9$16%9~g~Wz+zD>5K2A@B)WL7xnzbsT;k7vC?xeeZGJRugSm2#^C z4VQUB4RbflpGA*vdTVjWT&;dew`A7*xp}iL&zNAdEtZ1ekVG%^XQ<7g_CwDQ&UQhs zw2AAn;aW_e2f~V2QyTPcz>%m<48@s=>>_?F#F4};ZV-JoAJt1{c|yhh4T1GyaF#9X zEv%*!*pgYb9$!eDb@}3Xaoxi^$Mq*jjIzFX-Y{Pm&x;3*?u+N?`MkWeu(Z(SDqgX? nqC!JHO`A319y-SFp2s>K>wFAI5AI0=u@)1CdD``#NS ze`vMp_if$x^4)vRJ?GqW&;8wd-@EV4F0b#6typ4v*<+&Xd=VeG%nz9~?S4pVOC5Ucu>aPF~9CD<`d>@089OBmGmbD+Ddr zGc;!5<=^mnPd#{;*L&*0Bb=`GHndP1dXK8g8EgEz>1@~h7uV~uQ8w3_m2wVas~}mV zO3P=0=5T(qi3ss2jHPkW8OlZ>0EmA-+XQtzOw~#{nOs&SMeU4jg>}dG8M~W&4l?#Q zHFOVS$9aP!#Br3di$Hq3jFnLNEXOGH+mdzL;hP5dEXM*#?2w>a=|)B&Q{QPaBpp`9 z>K6nXJ(7p9-RDD5kCczJ8Y$^;VA?ED&6o)ovCT+CzlxM}I2em9Xj2UZiVc0`ZTgao zz&gmESGB930l{`83=eBVN;({5*WBc73^8UU%zBWh-LE4h9S*W9Dr#tIW(>oGD3J6x z!Rm*D?1G_S0|b052EmZsH;|GJ2e%V_0T>RG{3sIHZ9z&p9E>eXD*m5hpQU5CI0rp2 z&qnqz#&>vO9Wb_-?QBzuyyPm%F|T!`#~ zbCC^^gBwWuJ>s~Xi(K7I+HX^hC}ln8A-j@fUnM&aIjG7&c8J_xM%)icQA5U;5?4%_ zlfre7>>A3xMa9L$Jxl&Br*DC*MA<3o+pjQ-hWC;7IJrMXt<9$ze;`hw?DrQS zdyq_qsaQZY0`rmGo{sE4u|$TesqQnRor~Ey{4?TesS8h_8VA4uN7cb4C0ycwWQ%RH8oJ8{RgGRm{546I z@BquZR7>-LwbWVFC34lxJ*rZ-f+tl1Pu8=9`MnRdj#i;5^Vf8*2pWyuuQlf4Vz^TU z&}`Qd*MsJgpI_bPOHAb<#<3Gxh+vJ+wf z?h}(SNw-7OC01mT?sn)bKaX|Aq-&?}x=z=ItrV&?J)A+bz$9JD%OW1YZ$THP zQwXH_zPR$%wzlL}70P0XTOUS5* zx?iF=KZwVL$Q~hqwT`(#KRN>Z6oJ7#Uci@5krJH+q$7I@G*l<)L*w;H*ALlc%h=1l zhMlSaHfJw}HEAI{8=p7YjeDYbukrlXJpUEXdwKpS&t1qVny!P=;>epOserQcBJZ1| z%wC!|JT&E{cjNI9v+@iGCNY;)$b&?OFNINMSIly6A1~gGN>iaBbl2jN*DcY-4-?$E z4vks`dCV=JuzzCie|sI61C@G8`u>KU!i2E zZmiJnTEX=@aEGOQki*zs^GK7rFLo5iw?3onfE&k_xTR}V98O;*r}B^xDPfXLkO!29 zAlMR<@&$ReAU~r#kD^=t)o!djRv|whKNL6L4|giKaxE<`iI&+NHUR)yowt0t%PWC< z{*8pVW9tdooR9YHN4jEnrrKj8Rq=sta~dSdzf!-po(AeAk)1-(-a>QCKA=#C)o5xQ%v?rukZcNOM`kg@JY zK0zE6@~HeirbySpuw`*xJ=(sL=E%PR%jmLr-`m$GkQIMaDAJPWNM>9{d{h5&(K7unp7Ik7SIax0@VaPE!e zyQd9sL7EVqvAHsuy05ZyK)S>!SIiJf4+OKEvNK~^pHMm&Osk0W6^y)EA@}u&niiYFCk3=7OzZ0`p+_{~W4p4j@|zY5`Ic?Tv6BN)kW=Rsp?l~6 zkAEfN#|~jmLLtaP#)w%!aJlx_s9^bNwhzFB-zrwm!9AvtQ;A4ahWm-1;C|ATG%zC< zD6JqkGdXaV2luJPv5|CTF$z5TGd^%q-i}(drq#BzcDVl*t_hQ)S;`)@kgnVa zX5{!3K6#zZqd12wg2{M;spfHo{2ncQ%&lQw0}J&0$Oon{=J5;4*Wrh+Nt(wm@jVQ7 zCsk8T=;4a2sV_#!eK`bw03`=kgnW;By&GPLdWwxJFthC_O~Q%#1dRR}Mtr`l0He8m z*0Ba!tL5kBQ}qCR-!JGoWpVZ2W&S$xDt#0a`eSbP2HTC81UO6t0LJ-F2iIT zX%^(XS8l=iA~Fz{pH|n|(d(`p=rL6s8gSd6c6XN_MN%v|oQ`6{QaNgwX+BYhQ|u(& z%W?}v zwjNgM0D%1k5*dM@**miW1Cl*BlI1$3U=`J@*Kh5C+xy@4hKA4ih^Blth66! z^a3>f9JxuhSTDCcYaOZ5RN+?1G(pF z(MWvWIvA+fLADFB;p@{>i4ZmSq0D0d|H}kV*tU((*(oDcxlUzL5O31y9`--d?IZK< z1*`d?z4}@q`k2i@w|t$)V_3HG;a&9TT)7hs(?vvn=D8~(^XjP*z#O$7boLJ7p`!8x zY)KH$J8(NV`JZS*FGTMhf)aBM_oL`9Y2c(xh&0+tpp72oT5jaV6&$ziMQ-evqEUqu zC>D@UVyyOk@w}_K>#hM(M(OO|-9^n{twy=f*-MXkaHYpBN7r3o6)S}$t2MA!tl#Uh zRpsINH)u7TpW}HiZ^9HlZ4#o;(5xsQgcqr1l1#Zu2e<9L) z8VsATO_$zF{C1uomkQXpM>pMPg>3!%Ao$GSwwm4ru;YCCR z=V5znui18^dh2P~;gb9InrEU&)3x(NgR|1*lJ~phgU-$qqF?elJIB0!vBKFo>X$@c zdDfsyKH`$i{WXj^J5M^pQcHQ(Vfm0N>u~4TO3@#-?~_lvvJUbaQOl0*@==$(&(&Rd zS{~?L|91D<56f}cTP;MgA6bXh;fxHV&KtchZgF*+Z-vLDu{0Bt3YOO6iIOyDX&aFX z%{}jZh?_F&ZLky<=CKH(D^E}jMCs*>KV4%a1QMS(DvdY z!MfKTMIgsYPv2p)@8h9SM3tuzRQGztvitSv++zyrya7C%mff%BhWvZYZ{z7KHru#i ze+wMWQ(QGWw1zA$3c2CTG${1FW)-pqh3>UExMJkk(#q1p#j-hu8j*pFz2={Qp=V|9 zmMhk7^CKup=SBvsd(Gbgizkgc)o<^wLHI!=Pf*+&+{iG^7&()Z|C{9dA^0`?Md$Xua5HJuh5HJuh5HJuh5HJuh5HJuh5HJuh5HJw<#0WGs zS-wEe2XW7T^bewb;8=_A8d(MT z&8?N{wln$RhOkZrBx-x*&&T$V6+_E~O%f0TF=-xiYbO&NS04F5)Hbw{(E z&(XJm%iH9&`1h>JdgV3Lx(DCKD4O^MEV$s+?fW`Q@GXv*Wm})zH__;?0!t%E0|5g8 z0|5g80|5g80|5g80|5g80|5g80|5hpzYqcbT?HdE5HJuh5HJuh5HJuh5HJuh5cqQl z+~61SYUsAGR1mtt8We90dxK)5wV>S-yy}Vts9fo+b~~3Xsa(_IX%iclc>V1jpSN*U zxV3^Rg;;eUU~TpIH(3K=2?DDp*c@)fzjKGE^5puho(^A22y$w`ikDU277X~CVLPcE zI9nBvR*N3rIhG+k|JA-1 z>B)^q+FPu39o|mFa*?WiNc8>_?bxO>pE9*>W}E5%)h8V>Ri2~KN%|=(O|misQy{Q< zjcbjg?zGu$dH#kUyk98qufDe(ukB*Xm(;EeiNR29Q-dcY)w%)=>XN8kyL_1#+$j3Q zO|`4Vkmw0EwA8MwsIF}a20~I{qu5^9iV9+|R_yS!w)w=`pl8DduVgK>HiU{xuBmMn z{k0qMl7v=|KWqIB>$R;N^% zcpT}8IJi!4M~$^;I+f?*1yx$VPR_y4L7-gVRw=KOg5T<>g5L>swf+_*Xm%>gUX;nQ zgK4lndtVZMbblX}a{tp=cCqGvV!iYfmTo%tOq_XUjN5LOXX<5Yjif$Ih0Rp5S!ryR zl)+{#N@uf*vv|LF-zYN<1A#w*fPRlb?=YfsMv9i`Jx29N^xmSY@nYzI%wg(lV z#teX(zJQ;A0-powLaGK|4eCX5gKq=9AIS&)2cWx<@c4(tK<8NTb6w!eKz&HH;5$I? zMxx(@+6MXxQZD#W&|PyFvw=Sj+J!yA5aM|ZbP#C<m=VguV(ddsgx=o{( zY4#K2_{_@>t1XmhG+(2iRl7H7{;NOZb}rL!Wg1m%eyuIJsRDq(fc*JRim3V`subl+1u#OGqM-arm_Rqge2M9 zklh8@=nu}=EyI{7+i`99;ThR#j9bk^xPBzbf{?`^%Sftwn${iap6xKs17}0#lgW+q zz{kVy|J!+Bo4K`Pr6^T0_7iXwq2&Sk;H880@RBPK-ry7Q1WdK63bK1uZWUuy;FbqP zk0h#Al^!YR?WnuPUW8q+cSCE(f<~W@7151#c`#TRYOZK>`Wvh9Yp3%{))5!|}v ztJkiouBa@(YEcnylHJ1^0wFfb6!QAn)eN=05qK@Ps|$%Ob-3w=vW{ZS zXLSt$f0MU492D#Po?tL=TOB^n#nS`3TGNZqes52xM1Q*%pWf1^z4UXvydBLV;1@f* zQk~@C=#{a$pxBJC(T=8R(jPm_Wp!eQ*btUPU8PpK1hE9AI-m|(uChYzS5HlhlI z>^w+Su9;B>y?&T8Jh>=r4He$z^%ueeK2eie3Vl}X*Y} za8s2SYzhSN;dX<#JOC>UOHK;r%JOh9h(1>Z0}Wy*RME(Clcd!xm=cW;vR6pDW)phd zT8##rp%r+%LL58a%2iJ?D4Jj0|-R()oB$Wm$)I^+$;vG zg81+egZ6FnTE9Bs8p}Hx#5Q^kgdnM-Y_)lfD4kXPOWrrhgC^Q4Fgb5zZSb!l5(uuq zsz9WAJFR408E6dqM1jw5y!8A_<*QefyY0p5jQx1$D8t%7;PZmO)#%7dtuxnyUW1gS zB~5IQ&oKg-Ipe>#^TnCt%b(+}8(m96AeHkb?7{F16OV3nl`c<#02|9m^64-TpsN{V z>?RYQSE?0QnI{wyTQ~SNSvy*N{?O97;h=v>sG$Yl9SRk;dK>VKqd=2X*br!4;t91b zXuoPMeT~oCgiYo;^>d%4bFW%ZH22!%T=*zWlfF7e3CM& f$FpRNpYUk?qx>0P49~z(X4`YN6ETj@Hv<0)NQ~|1 literal 0 HcmV?d00001 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_RightToe.mexw64 b/dynsys_helper/Rabbit_helper/FROST/p_RightToe.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..15e3c11a0e9bc9fc0cc9b6c0a6412c9d990408a2 GIT binary patch literal 34304 zcmeHQ4|r77mA?}*Nl1tjFwrP#2OKnFftX=6kRUTKffqZWDFjS4m<-8FGG#IoXWqnM zT_qi|H9V)EUl;42?RJs2th%;M`&kLrT_=W?ApQx^Y7lKvarWQ%dhZyPd#v$*L&)LBb=`G);CiddXFm07;E^4sqFE&FRs^RBW$)cE9D%wEG76dcPLm<&urgLR zKiJ@rJdFM7d?@OW@{v{}B^?e-oB664GXW#E8HwmOkdh7uW3l16 z2l?}=c6HMr*p7tZVXa6>hlA{z8odo6#;k-{HxjjbJ5ti&AiKiC`o<>4W|CbDNskk( zemKZ37z)-yz{g?$4B34PDd})B9Yw|q@=^a*s`SJ|0(uaI);jJ&;#>K zWDj9{hZf`^`_2qx@8%+VjJSI-;6pEx$-Hc2-B=ex3oOWPqO6N#Pm=a2GX9SXkzH^u zvO#k2Rnq=|IBw@6S2vROyHq1eS@(I!t|Zwv$j(C!Dl?EBBKMaO_hV92lkug*6;bA- za2+JOhO+NaaS?IPlE2HT-Akx=F=ZD~c8dD;8_c4ieWX23?oUx`bE(Fkh*K#0;|0hb zAd?{~7Eq1ATx7SVBl{he$WRs4-A~%tn4LqvB(8?KunW~VutxS-BOr8D*>Z#~w=F~H zI6jD_)Pt$PgpIKjLB7kTfFHHwq?(kq{}YdoTBf^f#k#?iu?7oug96=PoQH4!x8|Wx zHyG!kQ8#et2B~A4IG`H{h{-SwR1@^mpCBVV%w~S?y^Butrro0b&@4K z!17Mj(tKbobyjtWTy=A|s?@FENtM8p^(tnnh#Pc54k@ItO=V%Is-ARVTbHipbe78#L+ZoWd*m}50j(Jd5 zCgT@VmAVxoRVCSv(KbE?f8 zuoR$Jt(udG+lCOUdv!F~nL)2kHI0!TQ&7a1BsQ;1z_T8$I| z5=L&@Yd8NkrWx9=84FJjy<%B`t)_`#?rF7x?Hw=zS8(YiyRjs zx7q4UQl1da`wA}lLNvPpMEP%A?1nFduF`b-xzGhv-JPU+4LUs#Km^bPd74St39$h8 ziOHCx+o96xUD|D8}W1TVS+DW{w)3sqMg=$R?XAmtgNtg1nNh_yI6ejIk(1ocK z=3>;G#Nl);l)^H5>Aq zFIJo%#^XX{kC4Dx$K0SF9|3-fz~CM);7g}aiOvAhkv#<(suT2~vHGOzhwQRt>}B7? zPE`P#vlqjfv=E++&l%~$J<*)kdH!3T|Ayy1Jb#4eF60zV=Rs*vK z56v4Mn)1?n@%XS=*$;wA%w^^B0MVgKVN}r>v)t3mi+7>YWM~LoHMrz;Np$hU1b41O zqgFv4b;~F0`;?nc#p7FI;dFa!h}OGeMS1Jsowz7Cc9|ke{Mhg^DRVb=E=+k0LF_eG zLqzc@vw2miN$G|htD*-)2~u{CD)>j@J(RuuE?MNgFQ8k|?AKSK_AG1zJ7eK1lnm94 z75d#PxPAxju#^vT7~5+eZdCWhj-vS1{mKrwacqfOI#O+JvLk!@9#3FL8AN{^=s>Cpk5qV@}Y<>>ySI4QSN;w9`CyN z805<3-0}tGK8WlG9b2F0+!joNKDkfOJLRDM)P9Wa2=X4GtJ>=Ba^!baVtxo2>#pY$ z#8EDf$RA*ebRG;_7Uk8U?K^0W`~p};m&GHeO}ETdMp2Llajq)FEZh=B32@6V0c+(_ zY+!Vi)RqvRkMLD-ByI{{iSfT2I{QJY@*%pED7=B4MWV13o;%}`ITD`|o}2I$gGr)^ za}#2^Cs)P8Y4#(S`-Lhn?ZCw0ys7q6kFqwdK3Y$bO&wf8zBr28;Bxu2JSxaTLg$gN zIlA05JcH)pij&lrz4D8K{9OEyHb_32d6MlfjMUH(ocXb+6LTqt&n0T-WkEiHlSSC9 z43Db*;rjW+6e#9XU}z@xs^d-yf^7C|z&eu;$o+!->BwtKg+u+?Tye|hGOp`&$^*L{ zfHLy>(kq{VLAgDaG@k^yS0*Wm=p3eWA_lml(^6H^EsVR~S?aXxa%JRJM5p208_Rc3 z>F0tpAv$eyMKpC^MM=MOiBm40CX^fqW;tbN#*|*6WFVMU9_cL@eyv>Y?Hki{qBlb4 zzVMWySYJ=Y4U9D|GKEhHXib>X+fhu9Xu`*KXJO?xE)w!B+md4^2cjUS&M8Fq&;cI* zO2m&H!mNZskcEt4vw+}o?XeNT^7CvTfC;}{q@IJjO~a=Wk*Ey!6FB=G$x=n;t5~62(;H11AwP;PNZE5Xr|IJ(zCPy=rJ!&CcxgN~$ z@kxC0I-5su4p|73u?CaP<8t|ZTKJe-L%aqS==qTkO<~OA7nIxKhp$PR$1m|c40b1# zlTGNM@~p`(M#+6S1b+Y}2UmoAk9xfuT7Y_rjVmy-?I=yaiTVVL{sl&SzO4YGxqa5L z23o7-ar3D<0KV^6be*!e`tCIU5oIO#UjU*S=D6Syg1HLSMPVxZwL>}l*494wwB9&j zV9)@^Ypoz-OMBcQACA0lvV40d{3}^-f__|yMSnT^;-;o<%uEdUt&mlcY$carG7dKh za^9;q;d~M4kIPT1>+I;#EBm`m<%jy+_NU!lWk-<|OAeG$z?c#}|XUI+%) z?GvvOnU>SBh_Afb9(Cn8^TSo{E1$OPa^?!rujLC<2Eg)#?~YD;M(E15m6sfl^5yHM zxl4M3nes9$eRoMLh!e%r1;c-^9~5x);R6-9)yDpwxJ)d(G|&{j4)Z4)r^))G&Wo*w zm0AE`KZw!F+HxeEE_^%hdl-g^L$2CyUh*?&+1A5S>Xw7ybFcw9`6uj-_A71>zL>_( z6tx~kH(o>NAI3@2o>u}V{4D#75cwb;&ZKKX1#Sg~ErNUqlVc|IN+AUHeVsAE;@Xe1 z>%@=}-GcWA;T^-ly}yeKUXu5Wa=Cx#U2NBAml(}{3EH9GqL`@88CpY%d>o^8YH()k z)L@*Sx>Yo3o?EqfZWSV8J`;XbwBrx+?!S_{WS&DkE9nadglI*6Ja5HqBrU@|Us+B( z&w-L06Rsk@ROO=;R<*p4%4OK*Rpbc0N3DV#yuB<(u=fhPv7N&KPnV)#*BdM80~)V03IqTyI@O# zc;10q!O8zj8+svn&mfeTbGRQxe@O!;r9z~^Rt#bddwCP4@M)6}g@$f@9YVBh#ImiAc!3&y7zWguBc8VtEUrqK?;V*Ln`N%y z6XW}~0cxXT525wRCdJrgan=$>g1SY$q=y{QWz9;thJZDmmx7qybJ=Q4JvhTN%PzOA z&QjKkc&N^VIZC@3T7T5Bo_4~fA0xugbJryHGNWVnGH)dIGU35U&lb2nr|pGE&uK7h z+%{oCA24t06ilZ0oDFDAo6BflkWC%M^`Vxd;fo^e`KjUaWWgHkIQoI_KO?78+AJOK zGspcV37dXCb(NpsF~&sSiF-@FFBPH}-(E&T^>=ry!1?VwK`s%namN|&1;v3Fuxp6) zWe6p|5575WqdEbl3431nUWWTd_+BoI_kHY-pVW7J&XRl4A3E3N&~0(Pe!`203eLm! z*j}^kdiB=Rvco0!?ln(Gk)~_MiF#*+%OyYKk`FpNPKbWV>+Bfy`o(f*$B17NePvk# zF8PQ{HuqIC=Il7>3`@;rS%>9AuB^iyqbo&!*uGCb?aDgHZ$vFSy2?ge@;+Br#c8>} zYyCT2Yd~!@*z?dIkh1)+FO-GqCe*wJHOsI;xuD}UW*MQuwbU_ex8WSx3!v@6MS^v$ zJ&Hh%m7KoKX5Ys{qlhX_BdD(RisgwnrgD!dsPiW9a9W;tEjQ%fYkmh$XR+DF4g1^R zaGv6-nV~ggaZ$((XQlz6_jRj~H6V1Y&A}BT$Cg%_7A}&_G1Q3kXY4iq3=BOhd#_Bf zc9|bWNjf*uZ{2JD9$2hw)@9xXj%H3VW+m?19iRLvFkuS+RRTVAn6viSBJ)DTgf+-# z@Y6J>@jG631u3jUq(^+TlzgJ+-RCiq8*xA1Kt8Q+w_*TL{_lvZ~%+xZ-Q z8@Q}hUW5KU%-M3UfsU0qZr@fh*`Gv%Dv-_{wA75A3Iwbz9{(n5AS^*(^#q&3E%RW;Ve-mscv;$|W z0@7;H<69@zOMxKw_Ls8Jw{%o(&tkl69W7K2%hEO-+;N zui1!~B(!)WZ+)nywRW|)sadKDi1S;-_FJxAh?jcf^;-PMLr#efyb{?s3nI@LG3@^FSDc)o}-87S#&Ns31zoj-lJuidhNmE%~LpsY_n8xyorXO|Syt^C5FGx>MQpc2PVyP5Q zZU)Om-Q22lmW%qiMLDh1!qf}X*o6x%?84xAM{`@xt48`vWqnT9O*VcWPO?5~%+0YCALUZ1Gn1A1D$ZP>$-#yCDd1j6^>e@t=c&wk`dgk=Ar-K4Z@y4T2v4eG6#-ybJq- zZAf)^Ao39CA?y|2f!vDg^$w&jL%s)eH?G;6!Jh_oUyO4CcpvD)NRNTfxCFmvg>)Mp z#`J@lzJ#BE0-powLaG8^1?oj|gKq`B56K7qN1(fr@c4(tKxbL;b6wy|L48Oy;M+m( zLZaV<+6MY6QZD!r(A~2bvw=Sj+KD~EAmVuxbO31@NCV*OKp#Om27V9d*15nE{Pq&yjFf|Sx6vN$-;r#HhxT5-N2&%-d#0J#=R69Y z_B#%wkH8y;fxvhKjywyfd0C@-H2PDGZqw*x zn*I1VKKJrNY7500&DZGX)$R?N|EkZqoy#;_nMPM=v|nrYm_~PMG+)!#Y2{lrTB^x? zTKO)GeqQ*D_j~kT$4frj@)Mf>ZjJ8L=zSX9s?p6F{cKyG=xt=@8QF_yQ`v!QLXzwq z$nJz}62d#;y86TtAXzLC9i|WhB)-P3sPI&vqE+fwLj=>Ey6heuK})3hBPOEEuc^HI+9w{S8(4wO7)Ls+wi6+S=u{ zp;ocp+vu&YZT9#Za1YMd9LPgbL#?!_Rjl>;8w0fsVn_-GHr4vPA?!V`h2NHt2yWf- z)oWK(l~yCm0OeQj5=X@$|s1*7V}D-&+$Z(ck99r?>QJFa2CEZ%4BT_{Daw zR4aKndZny3C^jK%w4-Sn^`{QAS*_SE)`ulgSE-dQK`cS37U;qeyI3#P;sfgXji>@4 zI}cKoYhu(vuOH?N4=(1lgy!Ai_0NL`e4-{b&+}b1@2YtkI^*>CZDh&YA|{&@rhY9f z1~*lT!Nx!kA8yx+%LA~&u;ip*t}G7+gXnW*Fiwm;Q_3-1DvWE~z)XNU_S4kWl5e@E zRl>+D4ViaTI8j=G6yU7JEW1 z^V_bPO<&{lHe!>xPW{+t$?U7<7tX#mJJV_{m4e}rL>Kyx>CLXzL(LHOc0sSSiTknQ zD5VdW(2Cel8T7VcOVlKW63S$C8Q&ISOX3#WM4#11`I6b5P`SS?uu%-owuZgVdfI_4 znce8|g~Zv{mM%#69$Pu#KgnW3>!l0E#9F!_(P>mKU7+K6ZKbo)>2j5?T3KDKF`uT) g>hLTX<0m{)_XvN+7sE4fl-c&2?L>^@i;ckl0>nG-{{R30 literal 0 HcmV?d00001 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_Torso.mexw64 b/dynsys_helper/Rabbit_helper/FROST/p_Torso.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..eefacc80f4e33224bc6e1698d6bb7f19f2b5536c GIT binary patch literal 33280 zcmeHQ4|r77mA?}*Nk|A2Adx6oA2?_zf`MT*7|8XhgSy*lnn^eKBqugMaW(JNrBLy%{Ed zw7Tf`RrkGo_nvdlIrrRqfA_rm?t3%yQg*{`HiAlMUPKq zk4!!?r_1CzGN+=^6BL>Q{)T|NNvL)Ee12KjED3>-Pw@Bz=XL9aCV!o@C_6jTs=M|+ z@Yr_>4jdner#U}cGjbWqGwwci`m>zAaC$zcFP^@b(_fz!KwmGOK0^8@V7Cyo;J~pF z3opOO>pgMzabE9*Amt;iMM^sC2%ANk88ZPR)`mp19x3UtGZtOctQm^bH;k3H*_dPm)=vIB znqAEl2<}C~^e`_{(qSjNhI&t3kTHQU+lxf)-hz~L*vYQ2u(rN|u~}pnMIyUxNJ)pC z>;l0+Eo6Ky`oWOhSCNtqJGYa(ei#mtd;p2;+K`eCJ7X)8ihon=VLAqU_6WMOe!=MF|GI%v*9+KTd+HaHbx5(97#C4H_ z%SroH;<%laToohO1_M;1iL%`@kY$l<4cR%!L7^Gh!{k1VxDHYjk?|zrrc*YDx+{=u zCS^V+X%+TqrM?tVsj!M8mYmc~wfoPp7`W`Z4UCOgzR+ta=oS^HRTLGMRe)kj z#)xGjNzb~1#y(=1mTHQNOk`}gTpt}JMJq0?6_ff0y_LUICR!O)8J*d9)*r8_H6ex% z6!j!yUlbT44Nv^*N^h{{(_pnWv14GGj$Aw7iY*=8*; z$rtmoNiV0%)Xg@no1HX-yHRrjhhKu2x>;Cai#`sX?W602S`Rip%NSIJb)(psnmMZe zMn25Ndi^3YDx~2T@lXKX;wZe6vHx4(5n@gr+XVxv`Bp48WLAej5F}hys`L{byckAh9Z}02-MqL9l_tVj?5x7DJ7qe# z5kQU&XjBlD5ts6+txx^>+p*a8Xeiwl9aJd}>?rR%w!>+)+aEK9SN#1KA(S)srIYSC zbmE}76(Z`Nx{_Cw8`OTtapD{UQNKaiPc*^9#J5v6l?(FDcVSqO?3B-;_8eSfI-;TZ zYJuiPK=>ZVRtft3BJ}TIxj$&mLSs00n0-lX=acIGGqIR`d(6_YHU_6}qa-TF#BdRl zt)kMac0;f|D(8#JEKzw<{UeGl<(H3B{Zi$r*t0Qnmj~wr2Un@0;FYZi<84^8!UqJ$ zO=o9tA{;-gEP;Fe^@O;6=c|gj2<_WWbVP4WwMB=@W4)c`OCV97q;c&$1JsMcD`r8G z5{KLkjr!P|u~_GYsbsmDTRyFJL1a5>-}w~hwj&67lpZlYC_9a(`U4C{R1S!p8wFRV zJ-@RY@gZhxxSj`yy;K=i-bRRY91U5P<+U|3b}PlmkAY=mWh{KgwB@tv2nxzET1yax z+Y#&xa4XLPYxQC*+i{iLk`P~s?rY+3%oJLH`M(r8+fk}A3%W$%rFWAGJK?z_CY!^t zNukdsd_`fBXktb}O!pj`cqq+w0Xa!~n|s2qtst51@bB2Tvdm6;km@lGs;z)QqZ4v!^j z=LJ!D6^lj4tY+!{;rgjW2o&)U7@YMNUOzc@MaA4M;XG5GQF=w&{_x3b#Akc;SaB(4 ziR-!@O8-7PpbY=^ngxeoP-=@N#gnLXDaKn(vy_%LQp-WJ+6EB(<-sxL5v+Ln)mi7C#fnawv|BN!?;`e;}L#Os-;mogJCgxlAB=?;h zNTQNDuMoq-0C@i!(0^Qk=Oh%OB4!MkMRYFL79AEX|B<~5z=Zy|L|cRRnugv?^h9I0 zpNlj<>FOn~VckAojeuZfvQMGKPEK7C9ZFZ1p|ICPSS2BP)&~~lC~DC;t+l1M!~O5z znh-gfuAb5g>FO3RL$6HWlh?WTG}e#`n2a`>Q|Yt4F57-rXUPDPYRTbr7#o(d zVaqi0G`JbBo0dHp^0U0b;j@5^u7;?T#>DEs*^PH2h zSZJ$QYpwu;>-IEGrsT9Q<7ZxVmv-hk@nGFLNYUzHb}yHn}v5Rpdpz{(6(6I6Y`6Zviw+H{jDS| z!97(;PAsnmB_%2@CcaAJBV~eCuAp)Wws~baV)uX`+Q8dNaztCVxDVSo9Po583buG- z#XUfyTkJGHa}zaEg?-wa{U{!yB2LpyA9VT?W2Oscju>9?#A?iIC@q%vZ9I57yW38H z1mymw8jZx}wZK3VLv|dp!8Le5NfI$)K8Z5#1Nh%1c*1t~A8B>UNL8yfCI$UXTJB+g zByJy`_Y<(XAKI%|ijf~$?YQuZ?XN^}+A7EQ;rRzsZ$-m&5mBBzGCw@8jVb}mVcSth zS3jPdsE@&x1hKqVzXDGArphhuI0hwR4)=Q)FKOUpxfrgq7Qs9+z_r}Sg)2C2iwj-Y zF-0OODNrmRpTr2ZzF6J{?z*Fwlo49}J3FWuoU0KobadTGt%jqx<>=S}Ru!mQidF-A z#hQaoYdIcM*+DBzFo@;7ji-dzQHl|0=+@LOMoNY)dwPf$snI?dP;2&B-Y>!8s+9T0 ziCNJ(=50JM?y>e$8|?=Otq&Izi5Y)?1~Wm!qEXUAgviQ9wNytyh~>>ipWbj_OxW~dM)*2+WnwQgGI}rbQerO?Iu`EQ4!2WV zo(^}N0mH^@6UGe$aoaY8V2aJV4XtTvYF|`L?M1c0rh(AxaBF^Q=sZOfBJBfjdw(B3 zlhR^oe~a1gGRfHV^UziQZBo2)QVV1DrDEj5wIWT`-(6RV_3b=SDHgGD$BOr~T7y1d z*AVW>5R3mi@VhY^)p024*IRzy%J;nRJ$()LJ>zp@eW!kC-?4d{?#6hi|2cG9oF8BC z!jg)0*cLr#c4Avew~m(koJ#jW^I{Y!T-#r*b(A@s$|0w6)Y1N`4CBsgo z&)HdaM(OR`^jc@-yCt~ntrNr9eP6)Y;RyGp&KtfaW^s0!zYdQpqG={37p$nk^Co%D zie@4gnD@T*E^f+%Ewe=IdFT&FS-#s7%tCb&YF>|;CD@>x-+q!=hG}vwH4NKrIEQu_ zv|V(I-&r|;j+`n!v(0Mj<6Wa3m7PJSIyb47$6uPvJ*J?JnMP-M{A6y>chH=Ro)T6E z%~Qc)o#Lul!S!S@JLrNlQ@_~#TS3g~7dtC+aK*^6rY%nkEm6z^8*%pcW*js>4F*BR z-Y8LpPIDhh@{Dk=aM1iHSe)6c)BFfHiku=uCGOkpAO0$k6ujra@~K12+M>(M73dSr zK_0>TDW;Tc)14Hkd8sNk8&R?NP+UE##n`<4t@LE*FwcL^^Mb7;xq#=7@cfrN-^=s; zJm1Ch7M{2A{05$H}28J%`7piSN`#4cjOf2p9+$2p9+$2p9+$2p9+$2p9+$2p9+$2p9;QGXnMX zmW$~5Any4G&e@saz(Bx2z(Bx2z(Bx2z(Bx2z(Bx2z(Bx2z(Bx2;1hv>y%OK`5k%$X zon_j$B?iB8F#a9TgialXY-bt&&iJOms)WJe!`uWvlz1867EtlKb6&}DZR1SZhRl3aO@Yb;DT4T^|cq_TO3i#o^GXktkEX|OQVwp0tNyG z0tNyG0tNyG0tNyG0tNyG0tNyG0tN#AM+EqH6^zV4z(Bx2z(Bx2z(Bx2z(C;K5xCAL z;nlFsA-NzpUkFHF3V8xjolwx?4lJI(2=2=q6)wlhtIF0lx|^lCt319Ix7Sm*Hq=x~ zm4d9o?-!cfzO8~kBtsy$0}Y`j{Mo;dRnQ=yHu(eJUx9Z^l5qtkKv2`i1WG6YEotXuC~zo&TJ?3eFJ< zc;lb&o;kMas;bJM6bM$;*SdppmD69Vg>qHps+CgUX2~mUty(7qC3m2VdhnX+@|lc%4R5rGA11tQC2dJc z`7}!!v(#lU3+h;wWHra@T#?UM1=4*J)G?)+SSs}=H-qJ(Zf-?7%SHX%B{|LHfAR%s z?1H5hc0u60f!yX9HF2NE;V*nXW50k;qIB?ntV=03@qVNy`oVSacC@}0PNwo)#^U~- z{S0F_U4*?4UeQC8*Ga*<^~>RROkI7vg$bIS%CeVcvg|+_tlxVq2|savACz+clUeo> z-T&Bn=_xGTG~;YP^URpH$621Ki)l5I#xM~!6UnBhvFUOKo4z!iO<$75$Hm7+nQ<5h zybl8Ldy3gDh&$M&=qUr`9He}tE3g6mmmJ0xfWH!nEJyDX8McephJ3ZM$L}M)-s6?x zH*D@^PYX7#q%n?96at|;@f%DW@wm8L7~TE&I~9*?5{}kCM{hqIeHgcYqw)BpmG|omnA?zE z1b;v1UDyK@!sZz0WfwpV-UZr;R0m!KeHZDnAYtwa&)BgMoZ0XerXW zke7qDBF%<;JLp|VR`B+4f-&W6Z{#_8Q90T zz%K>edNJ(49|p~^V&4URHfS?a75IBVpF?T|e+qQbJhTPg4!RR*5BU2*zj!Hd0^e2y zoROm7Y0vjfr00M+?d5)sl!3m{9_$^YD)6*_5|Dlf-Z%^d#v*X)Ax+Y&(?@lBr%t!& zv{9!OI=xD#IXZpq`- z5*=5j(`$9wtG9bfrw{5hU)R^@oeZJ1ult{=Zbo9cbIjwH!mgKP(6!}rIn@K3}rC+fZU_iGQFm94|P zZTt>ny-BhFWKqa6lIotJ^A2_2>oC@V_d@2}|Sm_DYO>SFMHQx9!V={YL>yOt5;PAo26P$y{ERi(e10lZ9QZ2AP>rQ z)$-P6soLYK_gB|RK{?>xTJ7}&WyY?C-=?4hZo{f|m1`?X%Ssk6E#ytI-K^FhWYbMS zkB?o!P}?Kp!>J~Aj@X6qQZ+uHuDuymAY|u3s&NgB2I%p@oZ(r>!lvNDEgs)O zc)%xfa^php;)RPB>gbHo8lCIy1Q8!hf0Y~s!Jg^~-%``bZL4rmzc6l}j z+<~pD++Hs}PoGqZ>C%8+?*e8b+_4{}u9Lm1+|4p(W|bdZK@|OjWG=t|=1_C|u~3Om z4s6B!6KOVh0&>Xht@Hs1^yZ6`N`p>mbEu&~3X})%Atom68|F%%7I1YXt+i4!Jy}90 zX`pPKdA%gRr}`IsY_c0oG?yYcuV>BhuOs3QT#K^;J&m{1MAl{gx{y~Ad3;~b*yl>t ztu1lcmS_?C;npa_+Cbpbg1{9R$ZCBsYe26=!v8bkNf#UBQ;a}n&gk#`TsCcV`BOY} zV`xbTq;lRwk8vrr#Z_^63Iy0#j+0M@fe78ke#UMv;Tfh@fh~6jgHqFG?^dC;$?FTQ zm>UZCt_s#R;yXyeg-xDXd~?ZPFE6b1H(lipHZ5vdJeR)4=c&ggbA$Ft(u%o@7ZuLE zIy+Mkmdk-qP^Jt02lQss>Y-*3d%J)~-pc(5IF{3AR%k`4D-U>DuqA4cf(d1^x`b~F zu_bXyEs|I8QodrYJ6P&#@!u>3<_aN?qn37HE9Ta_y+LX2)ypqW_#Rz3;Xlb@TdP;W<9T(tquk+imabjBaih+hOPSSZ|MorOA>p9{dPWe%^K%e(Z_d3$ KjN{Xd!2bfXaD_?$ literal 0 HcmV?d00001 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_q2_left.mexw64 b/dynsys_helper/Rabbit_helper/FROST/p_q2_left.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..9f95317aab4da6b9f742337e0a1eb4fc1d8e083f GIT binary patch literal 34304 zcmeHQ4|r77mA?}*Nl1tjAkipj2OKm)fskP}kRUTKffqZWDFjS4m<*GbWXfbF&b*1i z+Dbaf*6^HueqF48w%bM8vdY>v-OtuQ-F0GU3I2`XR*>38#qEosZU`#)r=9(s``!$b zKeSqPzpeXTzI)HP=bU@)xxahwd-uJWys5b1E;fZRmWmXMG1dc0hmGfdK06Eb1)tl0 z0efWXGjn*GH(&hfsoJa@tGZ8++c3@*Ncm?vokH4 zYwr&qeW2jLvC(*%^VoHxpGSG-{YOvF=k%r1S8@8A)0cDl@@X^Z+a)tbN&h(P7J?QW zI67+L<=^mnkKccc*L(c_}p8t%GE# zDlMB1n#1|+1|r0#FqXze=O`P203c?rGC*AiQ?*i@Of0Jsb1P#zVBPk8#_lAa1B^XF z4c*1qN!}m{aSSpx8)TP3XR>Y=eA57*=U60(Z4z{AoyaI;>N`z_q{GZu z-J(FfOL8%`_aZ3jkn)k%BPAU+Oq)fj88ZMQwjGJ+SCEnp8)MN$EvlhFv7xWLO>dGB zSR48CsCISJAlQY3;bARENr#Q>8XG+ILB`C4SvL~3`*ozG!$x+6h31yFf7DhJcU702s3S22#>t<94Fg55pmn4daWejoOORbW z6WLL6@I}&ok2r4UAXhh%_S;k=LRt5P$QF?7D`e*)2bCGfj*$B+i2ETas>%3r;)*D< zQ@A#gT|?QosJM){r^(-y)b3?eyp*!pl%1iz{R*>a=n!d7lKV5%+I*_<2jUdUet$8t z`^jX8iUm}|KOfnx>B#;QOJt~u>OMuBY#<2!V;|2wBgK-|d`A5w| zVccMxhlaR;EpCuDwu$HC29=1(VJR3jec53t8(Oz?1nrVSeuet)l{(GH#7~MbK#MUahgETnu-r z0GeHzCR42t7eMnMG^XpLydT8#9=DM5KUHZVqbV46Cm9mY4eQA8^|;|V&^jzV+$6`? zA6F*h7gd#UD@3YF(D5b?p_yYvjnU#_yC-_=i|oVP@6 z=6O>Aiq)z)nK&)zOLcD?O?Gb3t5XeQq=P)B>F1;dvLz{p2MQh;dW`XF8W$O2LQ@ob z6^}A0vU@gg7{^4Wc4SOsyft-6Bt^!jc_K2Q{Q#|^zW6$IVriKjT@Fh=Uuk@yg-;6M z+bnekDNl&xeE}DJA(CAWqWlpTyWvZqt2BIlCUkyPcL(WSfi4LQ^hA)S8I(N`3vi#9 zjDRn$+pXymD>6xUD|DuxM?0g^wUc<=6xW8#6skErltHw>AYIPO2CbYfQJA!EK^LY{ zn9EUf5{J{ZPzuYe(fzbl*hl*mB@N-=P&U5S3K|G`CgtokW)0BhoxoVHw;n%Wfz9uGf0^| zG;erl%8T#BV#7w|DG*FzE-RM@hz?y2ql(U`>F!=$yc3lsLqq7Q!6mOtqKh9UxP22E zH4E~nQ$A%qq}+Ta7VC(H(yh@UTJMS(< zL=>Mgmsgbv!W0OL;$sv4h6p26bO-D~j!SO4$uJwvL#ob6pHhUm~aSQ6XH+B#R*TD-S@> z5tZ@1zjmAj>c!y|?~CYi9CAA}$~|wzVqKS> zfLytfTRx}U3z7A(ZO5~m>%b)Fllz2tr)<=p+7HnkK|UaKRhyk%w*0P2%nu=B)Af9U z*vjP*`8`aL&ch+ovb;L9eLKyOe*>10Rk83{!!7fbQ558(I9C;77Is8X0^IV8z*@N+ z8<@CCYE6hQLinmU95aL#VEnIy&U%=typJv=3a?>jktpnd=gyd949BK~<|lkbVUlQK zWxgwEq3 zV`Q~qcm~bGHK(aB2jyQ2@-wj`+8}vp=1G>nFj7OuapuROPRykoK9{JSmjw9~P8K1f zGCZpKhwEn(Q=phnfuUL0tByM<2(r<&1?xY>Gp6(kB?E!9@^Ej#@GIqVZ{L`v6TJ~S z4~3=_Mf-XxZeXlonIUvqKx@L3-u7a8L=!r(FAFQbVVRI`+L;_XIS>UobzUL5hYs-g z*CKxG5auKlf-GbV8wCWHYmJTwrk`ee0Zi!4BJ~{HZ5Te2h(u+$pV%qxCtXPcGkmeq z41zP04R?8PpIQ_hPFI$p&}|^Bk`O)T11IHOs6}g9ZA)v1`)}f!5ILHm98e4C%JpD| zPfp^K*V#UTbI4Mdj5U~S9+%7S(!$5w8sasuKrafvZwO%?Kc{>he)yWCdHf>Z!(ew( zIoX6BD$knyVwBvMBk%`Ma&Se+cd6IAp~a}DShxZ++ltZzoTyL0=$~Q4=i3@En%k#s z8=y6ter`Nd2fz>gg0530N8cUB-=nMq{u@A4!yFepLNHgMx+qMAzcwg`-`LRypXM7U z3=A6Jcx@D9Y-vy0~Wk-wIhJ$<}fiCgX6U zAm_b&6V4am{+RrPy3PhyF6i$zlppDLTAy%sl?@^(rW{U3uwkhfF-LaGf^K^D z5ae;2*Ev5u`wyTrG?nO(ZFAo~BZWHp<~gk5n0*+EF6TStUTvuv4Zn@WLfZtlaVZ#F zw@Z{o^{{}e4{Rixho>H73d@+rm zDQZ28ZmfpTKZcW}HLnCt_*wQzA^ct}lu6fw3fu||n*{j?CdVx3l|l%thdQHz$?+7< zt`kE_bPL`efOiZB_x?66cuC$f%H{r{x3OKLU1B8rMQDd!KrvCBGqi>j`6Nc|%+XoV zGe=|m)UBdP^W3b>bF&Z@^O^9AqTRn+c;5o*l5rmOtfVjG7a|q;vAi|6k+cl=d}TSY zJR3@KR9HfMsmezx%xZZlmCLZrtH=?02hD;NytOPxu=Wc3u${vJPnV-$jW<@(2Q+$x zF5^#cq(*A6PdhV!;$A9ZnQs2D!=LCgT`+Tm@QUXg7}ro*EbsQO#y5mxPRBs*nHn?_ zo3{xDDt3_VgKX%!G*u!*jJ+uH7{LEB!4tME!*q7aNL5y!DcQ)!cPwKPe-0_V4PXX0TQxTxjp1$2_>w%Fv3$mfT*d1&h49yO=<)?-&lm&C7eegZ+e}&Jcw3^!A zWw!ea5;pyO>MB3NV~mNu6Ze*UUn)c{^_9_3{hfae&TkhAa*2SAJI;8|DK^A_T|>Ap zLn!%e;Pr7E)d?s~*z>}75$+q|+bWFred6~Y)pu;(ihIx>I@jgUZE=45gclYSoQJK^ zgGS5s>aC|~w?pndXq=8BP1p8QZhM8pAwTJm58K;Mi9X3=Zy)ve#BzK4h))u|Wmy9b z`M5(i_Ej@xZ$E7hNlj&0$K)f9tYht?YeiqkdPqL&$U4k#L`}Q9%0?XWAxBrmS-HP! z^IKgTKPbaxZ@m!CerOX`hdtb%I&WlU%;e}Y-VBc`qG<*u6|AVk6D4WRiWVZ57`xy7 z05@gkTV@H^^UxoVGJU@G?L+AfLfc z(43O9U)f0$HSfd;Y&N1o$&+#Qn6_f`c8BOG(Zf9d9?vscN%9`&cXECY&+p>-Zk~I2 zzKQ46JYUZ98lGRl^Cq6p=lNq2`TQ}@ck%q&Jn!InYm$5m=c{j}mX-+d?wFDTA+r;om6D&PcZX8TvMG zS&O_8|DIJ*r@V?~HnTq@L11;%x)#hyqLQV~s@v`b$0)Ag3Y$vn>XY2gZ zdePnGgvO_=2oS%T-47k z%4wk%re2c9E?H_~mjo^x%x#%j7x(!t{C$5mW3R&}Q95`W>r*NXJdX5499$P~M~$^` zDwXHrg;ZLwf+_+Xm%>gUYg0W18J~6e_s-Q;{HA? z<^HF#>>|zo_Qwq-c3a3j1(;iC|4rU`-`r|i=h88hp}ex+mOg|{C<&P+qfp=b4wn- zkN5_USB&5CxmrA}*us)VKmHdH2;GbSF~t!d*Tv?sjmO^^`OIeX;f5FJ?T%v)s;3`+ zy9jjS_4pYV;%BFB#?L>2N*6J96nu9+V^1RWgC78W11SpLf&Icxq)I#xc@Xpn_6{c? zH{<%f8)-J=2SE4X8r}l_EU5ERoFl+{K_5c;0r-r|V2k9%! zKs`vM;9EfNMXCY+1JHd)^m|WH&^cztEZ|E)y+}^*ZJ>7|Re|3L`Z7`~_z}>3bMUML z{7KMG>=F9W?!%x1NFRXz4QM*v%Rdb^xu9;ODCCcU9zp5>e-yOC0=!_;4SEEr1AP8G zykHXPPViNr_g#T_!Jh;zxDv5|F9p2|>AT<`1APmr2YhM)etrun3ce0>FVYL(4}k8N zkC$+Q-&F#fk?7rRv}gNwq+G;9d$Hdl3E*iDH4FQm?}DfOj}7T0c>T~37>~e-y{e>N zqd(W^y&AnuqfHvE(&%!H=4kY-hg3f=Y4m_bf2`4+8offZA0Nl3UVcz*p;)8&8vV4| zy+QL|^(nV=g@!BB=o*doYwe!U=pK#cYx+8^e5*!FHMv(S->K103!m|R2mfWf&~_4pe6we@093i!9xdObnxJ+Fn|=AZ~})9Upb*Hx8Qlr33W$eUz$F}FX+ zW*CAVAG?~NwnxH;OwH_?b9}AHRa2pjEanuhpe zh`Fp*Y!lrfNsO!1N|zy)fK&@~;fP%tFV*4$YWG%DfskDYsme7n>Y&F5bA~4w3!8%r zZ}IpR!UH~GlbRNKmn>YeP(x>&9>0w&d78y!lfu+5g~Y(NN-@yj58%UXx47C5D-26c z3g*h{P#}OlR|foUF&HebXSqqzswPZ{dI;IeB;BwLy>6~T1NPt=JZ>S5U1V^0f&`6f z?DT92xB}Z&yS%s=tC~`d;ZkAT-~?s@+_4|0u9v*4T`dwuX0;!oU>5z1WKO?-Yp5mu zP^Zi%1-9XyhBTWz0V(A2Zu9{NMDvv?0`^E z;jZglOF|%(^9Jm}@C=i&?QvCHo&o_jmJ{StVIV+PJ;2xv20RN^E3i^mFeo-}@oqD> zHG6%*6>~!Y-}0cl3Ew3OE^PL=@eL$@gS61?Z(i;SHZN*jGMB!_=V`zubCdcx(2BWB z78TCDHapX7E|mhIphOq?59!US) zZ6zJDH9V)E+r|24yIrI$tJb#Zezpedt`kE`5Yzx}1*vUR+`bs8+`RuH!CQs1Z67V+$T+L>;%jfe;=8d8`5b~KlKC|PCYs}65dT~K^cBVyh z?ft>y4;Jh_HWE*>p1fh?^C(~Vz|qrlIeqE$Rh<6j^yQqseA*29cFD96(mw^e`Je@R zkB*pl`8T}YQx6>D^`3g*IH#*U?j~v@-lNJg#_IomGMoPHu?=zAFq>`8O1XfsRgf%F zrDan=b2z`%K!o@d#?rXx9A(200K}DE1JrdeRV&5G__8W7w=yQfy6t<6-9h1wQ4Ix(4SnTpdXtR6 z+Q^?rwX2%~!A>L$4{Je6I&5Uu*x;!TGG->sx{#>duOTHJHnJ-$bT>3IHiPVe&y`78fG2-sUfDitfOy*}J>%zJiTxdde3uT=ody=$Ik@0_Af^7PQ z$c~bOFOv4V#BnHjiXqCOa27sLViigxp_2+z&`mO~#iKS45ee z!nKj?TFSmj#l^%OB!5>@yO&Y%Qp#phc82=)E6k$7{iHof?$1zbbE(E3h*K#0{d8mx zkjWqw3#f*FF0w7@$o>;cWUz|r9w6;(%+A4|5m!T9cpTL@uzL1MBOr8ES#pFsM@%sm`iMsY_n`tLw8u9s*V>Ce|3^2 zJizi!)za9rmO87tM6SBITUExb;7OIhlleShe)j{dqXL>TZi;(F&}i&Ft+7R140oyk znw^>^Q>_peK=TkZru9+Y58`=`TgdsJsN6b!qP42kE4b!7N@-0&P|+brGOB*)kn zS0>{ZRh4loM5;>A@g@$TnPXLFTE8EG^_dEKAaz<0){RaAxmqogmD55iKEHR4SIE|VU%=$$29ev)Ihc*kjp$q4x3x?P$su_BXnTc9)jJlYYJuA9K?y0|uErcll4p$wu02I+EMHfZH^iNd6P z3%W3w!d!xy6F8izg;H2%jXptJg*~)SQPLpZlL1cHv>(A?atNKNnWO4g^KKqO9qMK= z85L3&ixvC(v6v9vD!rh9sL@h()F2o0gL2A8}}i7tMa z;P!QB)GWv&PWhB|zjEuDSZrG~lx~d<(t20SC~rS{CoT%M#|`17K5TfIl(`2x7p6Rh zAodxnA)@${*}SUMsB}S&RnZNi1Sq>l75pRdF3R3~n=JC)70|6n_NyyVdlt5V9nsKL zN`~sj4E>%JT)zurPszr7v~NA$5xpbT8Xc;P^>rH4AW?ou{n~yOs27KqzAvK7amek^DEGb< zi*;Ul0&?X_Zuz`&A4Jx}w(ZYxZW|^+uiPudJ7uH()P9KW2=ZQ`v)b(JwB>hJVtxo2 z>(=uLVk?)2<@Yc}Iu3_Si}UKx_8l}w{tZ}0mc_zn4Y$ozMo^HC;#^gTS-35N65y6! z1lG#s*ucb9QfoqdF~V2H;g}&b595C&bk@UE<$ZK1QFskIi$q~NJa@z-V>mV`G&kWZ z3X?<=7be7XPp*oG(yYfZ_X|~E+JK48eoO7g9%XG@y|kVri#oW1eCbzl8(c1*l}7}5 zQ0O=wGDemghNjUxTydKEvQPfCAU_*Bq79OlW}amE3nMjn9A|zk>cm{i;d6=Fc}b8@ z;bajqDnlcxf4F`wF$IeG6d0U=z3P~gf*>1R8?nyhLvo*BeLDQg^}>-pZLTS-^gM>L@md$O?d8x{-srX9(#lLJwZQ|A<- zd*}dHgkY{hby1iMe{E0>y|KL) zKFv3e8yGae@med$*wUV~$;ZO)8cg5V4gX3OoS+}`u;{NuU!2s`&6$Y--vU`B$yRb1 zCgV_}Am_b&3(gnezL@;9y3Pi!pV!xAC_mEYv_9?ZEE_;lOgWqmW5ZH0Y?^93Rf|*X zINj9jLCE7auXBEC_8&lLXe!Ym+lJmfMhbQ0&2w1AG5atSUCwvPJ=#(;8h#s#g*FRr z<03G)Zl8LY$dsJ+#eC(}cB?DTo*$}m&U@PQxII^hd?{a;)DM;~d}n0JGeT#srM%>j zlrP^f#aYr5$ds31={rlJ0h}nFE*Scq^{{}e4`#<#ONP+t{wrE-{k*BD8}qpqQx68CpY%d=jH} z=ID&*nWHg&>Q>RDd2ZI`xmgH{`Aqml(XQXkzkeQe$vB64R?-{t3z3TaSl)`;Nm_<` zzOtNHo(&~ADqKx`smezx%xZZNmCLZrtH=?02F!vLytOPxu=WUhu${vJPnV-$jW<@( z3p9F!PUBB+rbcS8Pdn3(;v-bVGTrcDhd2weKAI0s% zbM67F`Juh~S|ReN#YVS$?I)vHw(_w(^ypl<0}azfL_YBBRpB{xR0&`XTMyg2`|(gw zc^tMRh~*vH0#5#C+RzJ;dyYbhIfwgE^p`YnQYwV&Eyd7A2Dp|RIdKKYZF`{;JEllP zAq9#B=oWDqMd#;O-bA%8=*AhI znI3mq>P%%lh==Noo1?Uwq4h@{>uw`#`YjUO(or1{}o3jzEX>%Fv3$mfT*d1&h2+a()<)?-&k_B_5ec(Ose}&Jc zw3^!AWw!eb5;pyO>MB3NV~p{>6Ze*UUn)c{^_9_3{hfaW&Tkh9a*2SAJI;8|D>lS{ zT|>AxLn!%e;Po*Z)p00|+w;PA5$+q|+bWFped6~Y)pu;p(tFV#I@jgUZE=45gclYS zoQJK^eMZZA_14q0%OUsdGfqX3rfd5tx4pvQke_kLhwbgBM4#lbw~u&yV!6G2*e8kJ zvaEiGeB2=$d#f3KL}$a1QNsXuENd zV4Z6R5Xgy=v$tEU`*~;-QN>vV)ww}2J@MLP?lA>*UI!j_(-W`c27UXCZ{bNUHru#i ze-j+eQ(QG8xP~ld2Ayzb=oflkH49n&Lg(5XTrqMiX{BkQBH0*4jc{MaKI2co(6h34 z$`o^_@nMvt3&VZpea3Ht#mZ)##vR~j<`iRA;=bMX(XRsIrtn`S;6sNwYmF{8((`Ss zK|X_@ra2{Nzp{fSYTk(x*la|Fl4s)TQEkQM>ld>zlLdA@|_H9Wt9=S@7H%kw8C^7&()@8tQnc)pG2tx58YoUh{f3Z579d@j#t z^85mxGvr-{-h*S)#CK|=x~&&<1at&+1at&+1at&+1at&+1at&+1at&+1at&GHUbR| zrq9vyLEQ5Xd~9dB104Y!0UZGy0UZGy0UZGy0UZGy0UZGy0UZGyfxig^Y-{mdBeNjC zvAsh5w#49uz47mWCUin z7kwgL4c!uw3W8Ue1LCbAPe80U7qq$pS6{UNl`HI3PW!SY6>FMYEn@u=kFV9`_0+El zHJ4MRAgl8G&CM>~X0tyeL11&E@~WDKfIleBuNPbAH=}|Ws1e&-%`INBCg9q*(Ic7Xo87^p;%jRfMPJP(ycD6? zC3)PznwHvIi)sU&#wKY&v)FdqHH+{PZ@f;6A9=_r(ScVW>xYhjj)0DUj=-mkfFaYs zGMC|{HgSqK7f&_JV1|ng?BcJhjZe+XV0qGHmRFz7@)o7ByrQWCHoOOnF2%1+$ImHT z!r1OFoD0-|x6z~(rBqHer7=@|1~Z|KsVJ)@US}OHIug>;6Vx%J8dxgDlbgYEQ8%|L zo#mo_Zc$DPwJ`aTG`YC@50~rW-|6Vd=jOD$FVx4(!k?LPsG7> z@pjZ$3nx=~HeN`j_3QL3{QLvT1x}UnIw|;Vj!O6)Q&;P6VS;9-vg}2fEIW_}>+|;| z;V16z!&2^lGRrQ~{Ew}dp2E@%7oLkV&xmn*g5?>ynOY;M4-;WCk!)HTnD*nw1u2OLz>1mtF1zjqVE~F~(J3wDXN(Da* zx@Q)im4H79+JQYnAKHBsv>)jM@V@~~$9wswz$O>ejTD9aNzfxm-QbUcZnFR{*mQv& zLD~jBe-2(SiF6nED$x6{K)m2jf)-qfSiqNp-i`Df@K1uih13l`wE#cAg%kx}2l@!o z3*h&HZl8;naDv}i0-TZP-EFjI`*);V#6x?r-y#X%X%966`=0NBr~Qu&=_Gjl&=DAm zz==mxNuNf4uF?B6db>uOG+L$6B^u4q=vxn~eqPe(UXA`(qdPQug=RlCj!(V(klI4A zM)NiLX|;Qk=D+GwZs!UOSEkVw8tv2CJ)zOv8qL@Aby|6gMoTrhS1aG8(N7DXv3>{s zb*$u*EkB|8@6zaQjozox?Hb*x(NDJZ@!p1apOgI>Z7REPO-Pcx1=*dD4S)Zf-7<`c zvJ=;aADok|#<*2Kh-+AqEC5*)vW%p*u{ zYaw>Qo{i0I3+laIR!H~RWr09Nu(7<}?yIlDuf391RMjkl)z&Vr4Yr7GPlLx@+vM`q zuffw83W&8n zS0LcOtrnl>;%S0at?9vMzgrS2(bww1r?>QJFa2CEZ%4E6`@}YnR4chSdZny3AT}aw zw4-Sn;!h!Fvs$rDbcZA{u2L&qhFAhpEzpG{c4@p+iw~&Xn@|Npb`hj1*T|@Y9v{pZ z9%RgK4$i;L6jg4ZUGJp>wF=*d1uJx%C zuD-0zEw<2OAp}VsWvh*AMCrWhU-G_5E;P|nj>);6wZOlIh(E9bs{)b6+i5223V(gb zD++vmkBTO9SZoC1l>*eE>UoPv&W5ZAo&}l`EGyn5?8Q!LF?7C>1%wR25d6d zsUHI^oqhF!!r9klXPV8WQXmwR=tBP?y;;?Is2RlGF5rB5BX z(UlYalPt!yUb=8ptfdPRoksQ2g>gKutF%|z9ggxbMo|Ymjdc5n`r2Y ziSh!KeH?B5936n39#TLfe-EIpX8=%G6euVp^{<76fx?19lED9`aQ-J;4IT1-JL=yA z=55*A*xcX_xG9X`mMOlc839Km2r2EC&Qy=hym<6T7kH0DF_~HuFHz8Q)UXhVJ;`0D zKvS12hA;Skla?{1PQwZur-j1eyq!J@CPv{LW=vcA7A%M~?5&2~z*aXxgL^P@sV~t} zklTtpl$}4tc^(FAco2FI=AD3CTiN+8)QzzEATKliR#gTThggddx}4r8N&{A!6VBOI{e*tLy9_T&dAIg!t|c?!I}IE18sL=he2fnymxc1T=c0gDN>w@k#MZ6g78 zRlHH}_@zn{w&BnL64=1;0&Bg*de20;HO6U6ONcx7vZ8xerV(s2+5Y#8xxN4X^8IZs zi{8C8QP(lqb%q;p`m0hFn>-Le1j-pJrw>9)&_>^Jfn0O4;1 zhdbOq5r4-+OL&in4X1h=`h#2~r<7x~|HP|GW;fjeAq!G%-q~R9 z0yDk@@U-`9%`w$)4Z@B<*9CzKzb1}=spD||>Hxl3kSO+6VYQ&$?!N3`pm&={zEOgen>XiF^V5Q6k~{WAXRGqxP@YFViDBD?}t zSHgqb`d7(Wn`zJeZIOV-Rk z;`Tlr9(R;nSg-*tn&^;~N~es=y|a2u#M9BPdBx>??KMNTJLt5Y$(S321(FiE*^#8A zG!@6*tD3NLQp1Rm@QRal9Qx&{ZsBqYQNA5ZI~h*Is|U)es(KnW?bE}#<~X$oo= z8KqrkZNHptySt=sCFdNhmxUO5BbF_%THEhb^&zs8Ju2}D{Bv#@R!8%Xt8>0L$!ZgQ zFP#Y!ftNpjaS&Dcvbq1fDBAKa-GS_q{;HyWRUdzDtwIjtrlAV|ldog$` z5ubj3Z1m^;Jl^8;h|a2s6jL^h$(3auiJu^v_B71C>z@qis_7Z5A_7 zSyA%o_EK<2im4+j*@VYXnv5#k6za~m+tWABWl`t?=!N|{Wv%&Esotv^<}yn_o7AqF ze28yAxuzDn^I4zCnBMJkSl9)bO0XqxG-O*LcIf=z@dm^|AFZCbVK_zdZnmz$dE3m9MaF9*tXDS3q=Z!RdG(9u- zseh+PPS7d!Q&6WQijZ%7)9^_Pe)bpFF#+!F9yX?_%}hWaT_&ZBeFgqST&9&w{#fK2 zxXbgQ5>C#!&#i6ixq`Z3g%{x5r>Pd+=d;rcWYObug1QNu(er^CC)J}=f9CgX6|2gp zALyisxw%p~%4D~3U3$3`2lBK{wWVc?0q}^v-QEU{7A3^*!1V88%Kw;U>1f7sHP|Sv z2=~8^h%B$EB5-gyE3%`m|K@UTf%qLs{?H=f6>?lv1@I?k>DFbp%HfMp%85?6qor+? zOSEynReUXj#`TeEBlKA7cEWP!zlZb6wn&(e$&4atpdLjy*{}F}e zX!7Y)g{rHolthQ>r;BovptA=aPcFnsfJaV2xpa7JD=lW-MYLo3 zMv>V#H~OMJ<;TV^o4WF`D2|Y|ZHj3PO$J9@t-{rYQc`X9`wCa$1lxB_&^J_t@9C=D zzxb*%8xI-P)n|dXYD2{z*Mss~!F#MH;-20PD%M`!_NF}F)9Sej83f)Z6-(b~d2~il zqomg=nGd3}|0sVm&s{nzhl;o^Z$r;hYc13Qv*^re29qePSBU{u+&9}=d%bE-DOmFH zr#E(O=AL!t%pGM?OPoFyXMKFJKEtidvAle3E`|z9VEVG-iK`YI7yt0>m&@|DpLBDs z0TrfksS<}gCaWX`k4C~JzPb-Z?1hTwWRYzKsGf0+g1U{lqHtTYELh(D4mx(>~|fMNwQJQgkhq3 zG0SUX1=eYo(bS)Vja5-r`rsjICJRIVOOpIM@hu*7;T{iaW6GmWY2 zSoyqJ7)9N$YfO9a(}(YCnQa2OgkpaAErb)s>vx?-YRV{l(_o$GMP?PoGs3wb=FX`{ zq}*duNl4(=FeDUSWXdbnB5ro@@SZSoK=7w_085=G$@Z)yaf2LJhIh{6C_@m1x>Q52 zq0oG@l;k5Vj@!(^-&AL=x|c3QdQ1b8*_5G`($C=I{z4ikx}iK9=#DgdvxI;?$JR*z z*{{On$*#rJNac%v>JNmd8#T8X6Y|-~&o*a;#c&(--6qx!t}Ys6sAj+*hO z_GY~Pic*l;HD8JxMSh)VcC=dOP+bBKN&!=CwU?7^hgg;f7Q!6ioIv0@JQEi}TfF_P zT#b0+H6OD)*8%(R3nNv^`?FVG&3po{WS^4ayO(q&nR7C@Hk{yQzX`~mTh z$olcrt$&6Em13V=;a&#{8qV!H*r0iG_TIi(wF$BVx@>P4eQPp#)43P7* z&vB13znBo@b^0CB_2?n#kcrRySEvYJ^cw++*&#rYC((+Pae=6Y5k1WucAsKD*md{( ztp+7c2MCw=gRJ0jI(;WPjP#+ksZ=cp!V*yAF`r^JOyIJ1^d3Zv=cPC~eBiJVE_gLf z1$XEYJx`cKJVWo|c8@uAbEP#hbBceUbV8dS1@U7 zy}k2Y{5EiUVMgULk(j4yA6VV7K{7FKo>!%NZIQ~;2Fn3)TP(>diuvCOZv=^ z=!jw#g zmi?WFmA)mfz!;srCWL-a~(((Lz(E#eyuYc|dal z>dEuCG-TVdD=RONYx>-<^w6L^gqqY@$1j3E=FZ|>GIW>;zdH)#^E+W+3x=csb~){mC&AHS(OEZx68_h2Y?EYbb(yvK!GtJ`?XDe2OI zojLXiu03;BR&kN0Uwxn``#glhyRY#!HvKfP9KaX*~EJuq>8st=Yl zgxxfv`b^lTYPXKNgR$swHbfWEnvc#N^(55EjMHymW9hZoUbG5-%BvDxBgW$%jOL!+ z5BQST-j_~d5g{w~?4u-`@WB|B4>c?CA7&t4NdcQp34DOlApV^ut}@bg z*xbj$wuMN((?Fe798GQV z&Ld9v~;rYeptT$nQzANK`Zgi#= z8+2GnO#{{mjI^4WDkRt%{76gn{bIEW{GrrcZ)Z`#OfJtFQ9j?kL_O zb!C67a2vVMmKoTNA==p&sPT6Hnjh5n(u^(5Ph5SSHOA*{-nY+FManm2dzP|$(vPLu zJr6S&ZzX0%{;Xgy#3AlybQY(p`Nr`48C!6 zAGj#rS%Hysy%~Giw9K{K-ZQ=y{*5zEA@yhRxv*}kpAJ85OAengR@MD`_YJtK9b~xo zF5uJv5e{`e0ZCE`*|j2lQjn0mJ5wyk!1^W92Qpe5%xhVzYnf2sqNyTdJj4gH1;kiz$#Ve_Axsi0o6St&C-Y+Ze2nOVlETTTC~Y zVN3+b8M%19BQvPKg4lrqf*s;5PDIF9<&t}>3P^_&2jb@~+#g!gTd5dfxhHw7T~v%7 zVnoM^eZgR!6z;3IVidXKKUkSk?K9G8B=64}^;Y;ah#Ra$<*WN*@ILL3Ld0m~w4<0m znEovV2PX(6FPHHYD~shukz<3Gj_aq6!yjg2F0rUBdDP!QSg;bt!Wx};xg(O+I+J4c}g~<<&fAMfiao*h$hkc`h30ED!mSYLASl!O>d(_lnH>Rx&Q8=H7xn)n2 qw`RN8>$W)UQkMtET{h*Y%cE58Zd29e#p5r1Th-;U0GW;`#s33-sk;3D literal 0 HcmV?d00001 diff --git a/demos/dynsys/@CartPole/bezier.m b/dynsys_helper/Rabbit_helper/bezier_outputs.m similarity index 74% rename from demos/dynsys/@CartPole/bezier.m rename to dynsys_helper/Rabbit_helper/bezier_outputs.m index 472006d..436d211 100644 --- a/demos/dynsys/@CartPole/bezier.m +++ b/dynsys_helper/Rabbit_helper/bezier_outputs.m @@ -1,4 +1,4 @@ -function fcn = bezier(obj, coeff, s) %accepts S in row or column and returns an array of the same orientation +function fcn = bezier_outputs(coeff,s) %accepts S in row or column and returns an array of the same orientation [n,m] = size(coeff); [x,y] = size(s); @@ -10,8 +10,8 @@ fcn = fcn + coeff(:,k+1)*singleterm_bezier(m,k,s); end -end - +return + function val = singleterm_bezier(m,k,s) if (k == 0) @@ -21,5 +21,5 @@ else val = nchoosek(m,k)*s.^(k).*(1-s).^(m-k); end -end +return \ No newline at end of file diff --git a/dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m b/dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m new file mode 100644 index 0000000..4696613 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m @@ -0,0 +1,8 @@ +function L2ydq_a = L2ydq_a_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m b/dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m new file mode 100644 index 0000000..8e90b63 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m @@ -0,0 +1,192 @@ +function L2ydq_d = L2ydq_d_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m b/dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m new file mode 100644 index 0000000..be8a6c6 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m @@ -0,0 +1,23 @@ +function dq_pos = dq_pos_gen(x) + +n = 7; + +dq_pre = x(n+1:end)'; +q_pre = x(1:n)'; + +% D = 6*De_five_link_walker(q_pre); +D = 1.2*De_five_link_walker(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_helper/Rabbit_helper/gen/dq_pos_gen_v2.m b/dynsys_helper/Rabbit_helper/gen/dq_pos_gen_v2.m new file mode 100644 index 0000000..d450bb0 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/dq_pos_gen_v2.m @@ -0,0 +1,21 @@ +function dq_pos = dq_pos_gen_v2(x, scale) + +n = 7; + +dq_pre = x(n+1:end)'; +q_pre = x(1:n)'; + +D = D_gen_rel(q_pre,scale); +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_helper/Rabbit_helper/gen/dydq_a_gen.m b/dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m new file mode 100644 index 0000000..c0628c4 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m @@ -0,0 +1,8 @@ +function dydq_a = dydq_a_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m b/dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m new file mode 100644 index 0000000..c73c70c --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m @@ -0,0 +1,77 @@ +function dydq_d = dydq_d_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/gradient_h1.m b/dynsys_helper/Rabbit_helper/gen/gradient_h1.m new file mode 100644 index 0000000..f451c19 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/gradient_h1.m @@ -0,0 +1,38 @@ +function gradient_h1 = gradient_h1(in1,L3,L4,R1,l_max) +%GRADIENT_H1 +% GRADIENT_H1 = GRADIENT_H1(IN1,L3,L4,R1,L_MAX) + +% This function was generated by the Symbolic Math Toolbox version 8.4. +% 24-Jan-2020 08:14:27 + +q1 = in1(1,:); +q2 = in1(2,:); +q3 = in1(3,:); +q4 = in1(4,:); +t2 = cos(q1); +t3 = cos(q2); +t4 = cos(q3); +t5 = cos(q4); +t6 = sin(q1); +t7 = sin(q2); +t8 = sin(q3); +t9 = sin(q4); +t10 = L3.*t2; +t11 = L3.*t3; +t12 = L4.*t4; +t13 = L4.*t5; +t14 = L3.*t6; +t15 = L3.*t7; +t16 = L4.*t8; +t17 = L4.*t9; +t18 = -t11; +t19 = -t13; +t20 = -t15; +t21 = -t17; +t22 = t10+t12+t18+t19; +t23 = R1+t14+t16+t20+t21; +t24 = t22.^2; +t25 = t23.^2; +t26 = t24+t25; +t27 = 1.0./sqrt(t26); +gradient_h1 = [t27.*(t10.*t23.*2.0-t14.*t22.*2.0).*(-1.0./2.0);(t27.*(t11.*t23.*2.0-t15.*t22.*2.0))./2.0;t27.*(t12.*t23.*2.0-t16.*t22.*2.0).*(-1.0./2.0);(t27.*(t13.*t23.*2.0-t17.*t22.*2.0))./2.0;0.0]; diff --git a/dynsys_helper/Rabbit_helper/gen/gradient_h2.m b/dynsys_helper/Rabbit_helper/gen/gradient_h2.m new file mode 100644 index 0000000..6e064c6 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/gradient_h2.m @@ -0,0 +1,38 @@ +function gradient_h2 = gradient_h2(in1,L3,L4,R2,l_min) +%GRADIENT_H2 +% GRADIENT_H2 = GRADIENT_H2(IN1,L3,L4,R2,L_MIN) + +% This function was generated by the Symbolic Math Toolbox version 8.4. +% 24-Jan-2020 08:14:34 + +q1 = in1(1,:); +q2 = in1(2,:); +q3 = in1(3,:); +q4 = in1(4,:); +t2 = cos(q1); +t3 = cos(q2); +t4 = cos(q3); +t5 = cos(q4); +t6 = sin(q1); +t7 = sin(q2); +t8 = sin(q3); +t9 = sin(q4); +t10 = L3.*t2; +t11 = L3.*t3; +t12 = L4.*t4; +t13 = L4.*t5; +t14 = L3.*t6; +t15 = L3.*t7; +t16 = L4.*t8; +t17 = L4.*t9; +t18 = -t10; +t19 = -t12; +t20 = -t14; +t21 = -t16; +t22 = l_min+t15+t17+t20+t21; +t23 = R2+t11+t13+t18+t19; +t24 = t23.^2; +t25 = t22.^2; +t26 = t24+t25; +t27 = 1.0./sqrt(t26); +gradient_h2 = [t27.*(t10.*t22.*2.0-t14.*t23.*2.0).*(-1.0./2.0);(t27.*(t11.*t22.*2.0-t15.*t23.*2.0))./2.0;t27.*(t12.*t22.*2.0-t16.*t23.*2.0).*(-1.0./2.0);(t27.*(t13.*t22.*2.0-t17.*t23.*2.0))./2.0;0.0]; diff --git a/dynsys_helper/Rabbit_helper/gen/hessian_h1.m b/dynsys_helper/Rabbit_helper/gen/hessian_h1.m new file mode 100644 index 0000000..34de3f4 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/hessian_h1.m @@ -0,0 +1,99 @@ +function hessian_h1 = hessian_h1(in1,L3,L4,R1,l_max) +%HESSIAN_H1 +% HESSIAN_H1 = HESSIAN_H1(IN1,L3,L4,R1,L_MAX) + +% This function was generated by the Symbolic Math Toolbox version 8.4. +% 24-Jan-2020 08:14:34 + +q1 = in1(1,:); +q2 = in1(2,:); +q3 = in1(3,:); +q4 = in1(4,:); +t2 = cos(q1); +t3 = cos(q2); +t4 = cos(q3); +t5 = cos(q4); +t6 = sin(q1); +t7 = sin(q2); +t8 = sin(q3); +t9 = sin(q4); +t10 = L3.^2; +t11 = L4.^2; +t12 = L3.*t2; +t13 = L3.*t3; +t14 = L4.*t4; +t15 = L4.*t5; +t16 = L3.*t6; +t17 = L3.*t7; +t18 = L4.*t8; +t19 = L4.*t9; +t32 = t2.*t3.*t10.*2.0; +t33 = t4.*t5.*t11.*2.0; +t34 = t6.*t7.*t10.*2.0; +t35 = t8.*t9.*t11.*2.0; +t20 = -t13; +t21 = -t15; +t22 = -t17; +t23 = -t19; +t24 = t12.*t14.*2.0; +t25 = t12.*t15.*2.0; +t26 = t13.*t14.*2.0; +t27 = t13.*t15.*2.0; +t28 = t16.*t18.*2.0; +t29 = t16.*t19.*2.0; +t30 = t17.*t18.*2.0; +t31 = t17.*t19.*2.0; +t40 = t32+t34; +t41 = t33+t35; +t36 = t24+t28; +t37 = t25+t29; +t38 = t26+t30; +t39 = t27+t31; +t42 = t12+t14+t20+t21; +t43 = R1+t16+t18+t22+t23; +t44 = t42.^2; +t45 = t43.^2; +t46 = t16.*t42.*2.0; +t47 = t17.*t42.*2.0; +t48 = t18.*t42.*2.0; +t49 = t19.*t42.*2.0; +t50 = t12.*t43.*2.0; +t51 = t13.*t43.*2.0; +t52 = t14.*t43.*2.0; +t53 = t15.*t43.*2.0; +t54 = -t46; +t55 = -t47; +t56 = -t48; +t57 = -t49; +t58 = t44+t45; +t59 = 1.0./sqrt(t58); +t61 = t50+t54; +t62 = t51+t55; +t63 = t52+t56; +t64 = t53+t57; +t60 = t59.^3; +t65 = (t36.*t59)./2.0; +t66 = (t37.*t59)./2.0; +t67 = (t38.*t59)./2.0; +t68 = (t39.*t59)./2.0; +t71 = (t40.*t59)./2.0; +t72 = (t41.*t59)./2.0; +t69 = -t65; +t70 = -t68; +t73 = (t60.*(t46-t50).*(t47-t51))./4.0; +t74 = (t60.*(t46-t50).*(t48-t52))./4.0; +t75 = (t60.*(t46-t50).*(t49-t53))./4.0; +t76 = (t60.*(t47-t51).*(t48-t52))./4.0; +t77 = (t60.*(t47-t51).*(t49-t53))./4.0; +t78 = (t60.*(t48-t52).*(t49-t53))./4.0; +t79 = -t73; +t80 = -t75; +t81 = -t76; +t82 = -t78; +t83 = t69+t74; +t86 = t70+t77; +t84 = t66+t80; +t85 = t67+t81; +t87 = t71+t79; +t88 = t72+t82; +hessian_h1 = reshape([(t59.*(t12.*t42.*2.0+t16.*t43.*2.0-t2.^2.*t10.*2.0-t6.^2.*t10.*2.0))./2.0+(t60.*(t46-t50).^2)./4.0,t87,t83,t84,0.0,t87,t59.*(t13.*t42.*2.0+t17.*t43.*2.0+t3.^2.*t10.*2.0+t7.^2.*t10.*2.0).*(-1.0./2.0)+(t60.*(t47-t51).^2)./4.0,t85,t86,0.0,t83,t85,(t59.*(t14.*t42.*2.0+t18.*t43.*2.0-t4.^2.*t11.*2.0-t8.^2.*t11.*2.0))./2.0+(t60.*(t48-t52).^2)./4.0,t88,0.0,t84,t86,t88,t59.*(t15.*t42.*2.0+t19.*t43.*2.0+t5.^2.*t11.*2.0+t9.^2.*t11.*2.0).*(-1.0./2.0)+(t60.*(t49-t53).^2)./4.0,0.0,0.0,0.0,0.0,0.0,0.0],[5,5]); diff --git a/dynsys_helper/Rabbit_helper/gen/hessian_h2.m b/dynsys_helper/Rabbit_helper/gen/hessian_h2.m new file mode 100644 index 0000000..57b2a47 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/hessian_h2.m @@ -0,0 +1,99 @@ +function hessian_h2 = hessian_h2(in1,L3,L4,R2,l_min) +%HESSIAN_H2 +% HESSIAN_H2 = HESSIAN_H2(IN1,L3,L4,R2,L_MIN) + +% This function was generated by the Symbolic Math Toolbox version 8.4. +% 24-Jan-2020 08:14:41 + +q1 = in1(1,:); +q2 = in1(2,:); +q3 = in1(3,:); +q4 = in1(4,:); +t2 = cos(q1); +t3 = cos(q2); +t4 = cos(q3); +t5 = cos(q4); +t6 = sin(q1); +t7 = sin(q2); +t8 = sin(q3); +t9 = sin(q4); +t10 = L3.^2; +t11 = L4.^2; +t12 = L3.*t2; +t13 = L3.*t3; +t14 = L4.*t4; +t15 = L4.*t5; +t16 = L3.*t6; +t17 = L3.*t7; +t18 = L4.*t8; +t19 = L4.*t9; +t32 = t2.*t3.*t10.*2.0; +t33 = t4.*t5.*t11.*2.0; +t34 = t6.*t7.*t10.*2.0; +t35 = t8.*t9.*t11.*2.0; +t20 = -t12; +t21 = -t14; +t22 = -t16; +t23 = -t18; +t24 = t12.*t14.*2.0; +t25 = t12.*t15.*2.0; +t26 = t13.*t14.*2.0; +t27 = t13.*t15.*2.0; +t28 = t16.*t18.*2.0; +t29 = t16.*t19.*2.0; +t30 = t17.*t18.*2.0; +t31 = t17.*t19.*2.0; +t40 = t32+t34; +t41 = t33+t35; +t36 = t24+t28; +t37 = t25+t29; +t38 = t26+t30; +t39 = t27+t31; +t42 = l_min+t17+t19+t22+t23; +t43 = R2+t13+t15+t20+t21; +t44 = t43.^2; +t45 = t42.^2; +t46 = t16.*t43.*2.0; +t47 = t17.*t43.*2.0; +t48 = t18.*t43.*2.0; +t49 = t19.*t43.*2.0; +t50 = t12.*t42.*2.0; +t51 = t13.*t42.*2.0; +t52 = t14.*t42.*2.0; +t53 = t15.*t42.*2.0; +t54 = -t50; +t55 = -t51; +t56 = -t52; +t57 = -t53; +t58 = t44+t45; +t59 = 1.0./sqrt(t58); +t61 = t46+t54; +t62 = t47+t55; +t63 = t48+t56; +t64 = t49+t57; +t60 = t59.^3; +t65 = (t36.*t59)./2.0; +t66 = (t37.*t59)./2.0; +t67 = (t38.*t59)./2.0; +t68 = (t39.*t59)./2.0; +t71 = (t40.*t59)./2.0; +t72 = (t41.*t59)./2.0; +t69 = -t66; +t70 = -t67; +t73 = -t71; +t74 = -t72; +t75 = (t60.*t61.*t62)./4.0; +t76 = (t60.*t61.*t63)./4.0; +t77 = (t60.*t61.*t64)./4.0; +t78 = (t60.*t62.*t63)./4.0; +t79 = (t60.*t62.*t64)./4.0; +t80 = (t60.*t63.*t64)./4.0; +t81 = -t76; +t82 = -t79; +t84 = t69+t77; +t85 = t70+t78; +t87 = t73+t75; +t88 = t74+t80; +t83 = t65+t81; +t86 = t68+t82; +hessian_h2 = reshape([(t59.*(t12.*t43.*2.0+t16.*t42.*2.0+t2.^2.*t10.*2.0+t6.^2.*t10.*2.0))./2.0-(t60.*t61.^2)./4.0,t87,t83,t84,0.0,t87,t59.*(t13.*t43.*2.0+t17.*t42.*2.0-t3.^2.*t10.*2.0-t7.^2.*t10.*2.0).*(-1.0./2.0)-(t60.*t62.^2)./4.0,t85,t86,0.0,t83,t85,(t59.*(t14.*t43.*2.0+t18.*t42.*2.0+t4.^2.*t11.*2.0+t8.^2.*t11.*2.0))./2.0-(t60.*t63.^2)./4.0,t88,0.0,t84,t86,t88,t59.*(t15.*t43.*2.0+t19.*t42.*2.0-t5.^2.*t11.*2.0-t9.^2.*t11.*2.0).*(-1.0./2.0)-(t60.*t64.^2)./4.0,0.0,0.0,0.0,0.0,0.0,0.0],[5,5]); diff --git a/dynsys_helper/Rabbit_helper/gen/psi_gen.m b/dynsys_helper/Rabbit_helper/gen/psi_gen.m new file mode 100644 index 0000000..5559643 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/gen/psi_gen.m @@ -0,0 +1,11 @@ +function val = psi_gen(x,y) +a = 0.9; +val = -sign(y).*abs(y).^a-sign(phi(x,y,a)).*abs(phi(x,y,a)).^(a/(2-a)); + +end + +function val = phi(x,y,a) + +val = x + 1/(2-a)*sign(y).*abs(y).^(2-a); + +end \ No newline at end of file diff --git a/dynsys_helper/Rabbit_helper/modelParameters.m b/dynsys_helper/Rabbit_helper/modelParameters.m new file mode 100644 index 0000000..f126231 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/modelParameters.m @@ -0,0 +1,34 @@ +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 scale + +scale = 1; % need to be fixed +% 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/dynsys_helper/Rabbit_helper/rabbit_event.m b/dynsys_helper/Rabbit_helper/rabbit_event.m new file mode 100644 index 0000000..8c321d3 --- /dev/null +++ b/dynsys_helper/Rabbit_helper/rabbit_event.m @@ -0,0 +1,16 @@ +function [value, isterminal, direction] = rabbit_event(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/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index a747063..1da5f1a 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -30,7 +30,9 @@ % 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 + config + use_phase %% Functions generated from symbolic expressions. % (Used when setup_option is 'symbolic'.) @@ -63,7 +65,7 @@ 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) diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 258658b..80f1227 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -4,7 +4,13 @@ function init_sys(obj, params) % 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 ~isfield(params, 'use_phase') + obj.use_phase = 1; + else + obj.use_phase = params.use_phase; + end + if strcmp(obj.setup_option, 'symbolic') disp(['Setting up the dynamics, CLFs, CBFs from defined symbolic expressions.', ... '(This might take time.)']); @@ -111,6 +117,7 @@ function init_sys(obj, params) n_cbf = 1; try obj.cbf(x_test); + disp(obj.cbf(x_test)); catch e n_cbf = 0; end diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index 25371cb..f6f0764 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -83,14 +83,14 @@ function [y, dy, L2fy, LgLfy, phase] = eval_y(obj, s) % Todo: interpret this and make it more robust! -% if isempty(obj.phase) -% y = obj.y(s); -% dy = obj.lf_y(s); -% L2fy = obj.l2f_y(s); -% LgLfy = obj.lglf_y(s); -% phase = []; -% return -% end + if ~obj.use_phase + y = obj.y(s); + dy = obj.lf_y(s); + L2fy = obj.l2f_y(s); + LgLfy = obj.lglf_y(s); + phase = []; + return + end phase = obj.phase(s); if phase > obj.params.phase_max y = obj.y_max_exceed(s); @@ -128,7 +128,7 @@ function phase_ = phase(obj, x) % phase_ = phase(obj, x) % For 'built-in' setup, override this function with the - % user-defined implementation of the phase. + % 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 diff --git a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m index abdac78..ba29b94 100644 --- a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m +++ b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m @@ -113,6 +113,7 @@ 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, [], [], [], [], [], options); if exitflag == -2 feas = 0; diff --git a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m index e4bf1a6..c992711 100644 --- a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m +++ b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m @@ -3,6 +3,7 @@ % Inputs: s: state % mu: virtual control input % Outputs: u: control input. +% u = u_star + LgLf\mu y = obj.y(s); if obj.rel_deg_y == 1 Lfy = obj.lf_y(s); diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index 2d6b4bc..41f03ce 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -1,6 +1,5 @@ function init_sys_FL(obj, params) %% Functions that initialize dynamic system - obj.params = params; %% Symbolic Function if strcmp(obj.setup_option, 'symbolic') disp(['Setting up feedback linearization dynamics, CLFs, CBFs from defined symbolic expressions.', ... @@ -55,7 +54,7 @@ function init_sys_FL(obj, params) obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P if isfield(params, 'clf') - if ~isfield(params.clf, 'rate') + if ~isfield(params.clf, 'rate') || ~params.clf.use_user_defined_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 diff --git a/us_Vs.mat b/us_Vs.mat new file mode 100644 index 0000000000000000000000000000000000000000..c319b1b592ee315681b81e34ff3f8e9dfcdbfebd GIT binary patch literal 329 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NSi^}l&jxdzrA1O{LC$nIBzTR8seQeFncUR__gUoqQufKm2 z(?5_om0)w;95>{12oQ0(sMz7Pc eventfunc!! + else + options = odeset('Events', event_func); + [ts_k, xs_k, te, ~, ~] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, options); %TODO => eventfunc!! + end + t = ts_k(end); ts(k+1) = t; x_next = xs_k(end, :)'; % x_next = clip_theta(xs_k(end, :))'; xs(k+1, :) = x_next; - % Evaluate CLf related values after one time step. + % Recording Section: Evaluate CLF related values after one time step. + % Record dVs_error(k) = dV - dV_hat + % Vs = V_next [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); - ys(k+1, 1) = y_next; + ys(k+1, :) = y_next; % Wonsuhk_revised V_next = control_sys.clf_FL(y_next, dy_next); LfV_next = control_sys.lF_clf_FL(y_next, dy_next); LgV_next = control_sys.lG_clf_FL(y_next, dy_next); @@ -81,10 +95,17 @@ % disp("debug"); % end if verbose - fprintf("mu: %.4f \t dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t LgV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... - [mu, dV, dV_hat, dV-dV_hat, LfV, LgV, x(3), x(4)]); + mu' + fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... + [dV, dV_hat, dV-dV_hat, LfV, x(3), x(4)]); + LgV end + if ~isempty(te) + disp("Touched Ground"); + break; + end + x = x_next; y = y_next; dy = dy_next; From 077d804ff5d7bf61086960c7cb124ee1cc34dbfb Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Wed, 11 Aug 2021 22:37:55 +0900 Subject: [PATCH 08/49] model parameters integrated under anim folder Rabbit designed more OOP way --- dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m | 74 --------------- dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m | 89 ------------------- dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m | 64 ------------- dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m | 28 ------ dynsys_helper/Rabbit_helper/rabbit_event.m | 16 ---- 5 files changed, 271 deletions(-) delete mode 100644 dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m delete mode 100644 dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m delete mode 100644 dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m delete mode 100644 dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m delete mode 100644 dynsys_helper/Rabbit_helper/rabbit_event.m diff --git a/dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m b/dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m deleted file mode 100644 index 0812581..0000000 --- a/dynsys_helper/Rabbit_helper/FROST/C_gen_rel.m +++ /dev/null @@ -1,74 +0,0 @@ -function C = C_gen_rel(q_rel,dq_rel,scale,torso_mass) -if nargin < 4 - torso_mass = 0; -elseif any(isnan(torso_mass)) || isempty(torso_mass) - torso_mass = 0; -end - -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 diff --git a/dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m b/dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m deleted file mode 100644 index b05f48e..0000000 --- a/dynsys_helper/Rabbit_helper/FROST/D_gen_rel.m +++ /dev/null @@ -1,89 +0,0 @@ -function D = D_gen_rel(q_rel,scale,torso_mass) -if nargin < 3 - torso_mass = 0; -elseif any(isnan(torso_mass)) || isempty(torso_mass) - torso_mass = 0; -end - -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 diff --git a/dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m b/dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m deleted file mode 100644 index 63a9237..0000000 --- a/dynsys_helper/Rabbit_helper/FROST/G_gen_rel.m +++ /dev/null @@ -1,64 +0,0 @@ -function G = G_gen_rel(q_rel,scale, torso_mass) -if nargin < 3 - torso_mass = 0; -elseif any(isnan(torso_mass)) || isempty(torso_mass) - torso_mass = 0; -end - -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 diff --git a/dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m b/dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m deleted file mode 100644 index 4120411..0000000 --- a/dynsys_helper/Rabbit_helper/FROST/fzeroFunc.m +++ /dev/null @@ -1,28 +0,0 @@ -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); - pFootSwing = p_LeftToe(qInit); - - dqInit = x0(8:14); - dqInit(1) = dxInit; - dqInit(2) = dyInit; - - vFootStance = J_RightToe(qInit)*dqInit; - vFootStance = vFootStance([1,3]); - - constr = [pFootStance(3); - pFootSwing(3); - vFootStance; - ]; - - - -end \ No newline at end of file diff --git a/dynsys_helper/Rabbit_helper/rabbit_event.m b/dynsys_helper/Rabbit_helper/rabbit_event.m deleted file mode 100644 index 8c321d3..0000000 --- a/dynsys_helper/Rabbit_helper/rabbit_event.m +++ /dev/null @@ -1,16 +0,0 @@ -function [value, isterminal, direction] = rabbit_event(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 - From bae0d60ac7c0a5dd949abd33b7901ff77a91495c Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Thu, 12 Aug 2021 12:22:34 +0900 Subject: [PATCH 09/49] Settled animation issue --- anim/rabbit_model/modelParameters.m | 32 ++ demos/init_clf_simulation_rabbit.m | 3 + demos/run_clf_simulation_rabbit.m | 23 +- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 641 +++++++++++++++++++++---- dynsys/@RabbitBuiltIn/bezier_outputs.m | 25 + dynsys/@RabbitBuiltIn/defineSystem.m | 26 +- dynsys/@RabbitBuiltIn/rabbit_event.m | 16 + lib/@CtrlAffineSys/init_sys.m | 5 +- utils/rollout_feedback_linearization.m | 9 +- 9 files changed, 669 insertions(+), 111 deletions(-) create mode 100644 anim/rabbit_model/modelParameters.m create mode 100644 dynsys/@RabbitBuiltIn/bezier_outputs.m create mode 100644 dynsys/@RabbitBuiltIn/rabbit_event.m diff --git a/anim/rabbit_model/modelParameters.m b/anim/rabbit_model/modelParameters.m new file mode 100644 index 0000000..1079683 --- /dev/null +++ b/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/init_clf_simulation_rabbit.m b/demos/init_clf_simulation_rabbit.m index e74ccc1..17e6432 100644 --- a/demos/init_clf_simulation_rabbit.m +++ b/demos/init_clf_simulation_rabbit.m @@ -28,7 +28,10 @@ dict.udim = 4; % TODO hard-coding dict.rel_deg_y = 2; dict.use_phase = false; + + % Uncertainty term dict.scale = 1; + dict.torso_add = 0; %% Input options dict.uSat = 150; diff --git a/demos/run_clf_simulation_rabbit.m b/demos/run_clf_simulation_rabbit.m index 5384bc2..62ab87c 100644 --- a/demos/run_clf_simulation_rabbit.m +++ b/demos/run_clf_simulation_rabbit.m @@ -2,27 +2,29 @@ % This is CBF-CLF Helper Version of RABBIT Bipedal Walking. % This version now supports close all; clear all; clc; +%% Animation setting +global animation_scale % animation scale declared global %% Set Parameters -params = init_clf_simulation_rabbit; % save - % Simulation Setting % sim_t: overall simulation time: do not need it in this mode actually % dt: time interval to collect data used for plot/animation % nstep: number of step that RABBIT would take +% params: all the system-related parameters +% verbose: speak about the CBF-CLF(1), silent(0) +% with slack: slack variable in QP(1), no slack variable in QP(0) + +params = init_clf_simulation_rabbit; % Load all the parameters dt = 0.025; % simulation interval sim_t = 100; % simulation time nstep = 10; % number of steps - with_slack = true; % use slack variable in QP (change this into false in order not to use it) verbose = false; % show the QP-solving process %% Initialize state % Init state. - q0_ini = params.legacy.q0_ini; dq0_ini = params.legacy.dq0_ini; x0 = [q0_ini;dq0_ini]; - % Relabeling matrix (reset map) R = [1, 0, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0, 0; @@ -35,11 +37,11 @@ %% Prepare System control_sys = RabbitBuiltIn(params); plant_sys = RabbitBuiltIn(params); +plant_sys.params.scale = 2.5; % Reflect model uncertainty here controller = @control_sys.ctrlClfQpFL; -event_func = @rabbit_event; +event_options = odeset('Events', @control_sys.rabbit_event); %odeFun = @plant_sys.dynamics; %odeSolver = @ode45; -%options = odeset('Events', @rabbit_event); %% Prepare record paper total_k = ceil(sim_t / dt); @@ -56,19 +58,19 @@ xs(1, :) = x0'; ts(1) = t0; -u_prev = [0;0]; % Todo %% Main Simulation for k = 1:nstep step_summary = strcat("[Step]", num2str(k)); disp(step_summary); + [xs_new, us_new, ts_new, Vs_new, ~, ys_new] = rollout_feedback_linearization( ... - x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, [], verbose, event_func); + x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, [], verbose, event_options); % Reset Map when event is detected n = params.xdim/2; q = R * xs_new(end, 1:n)'; - dq = R*dq_pos_gen_v2([xs_new(end, 1:n) xs_new(end, n+1:2*n)], plant_sys.params.scale); + dq = R*plant_sys.dq_pos_gen_v2([xs_new(end, 1:n) xs_new(end, n+1:2*n)]); % Renew the new state x0 = [q; dq]; @@ -79,6 +81,7 @@ end %% Five Link Animation +animation_scale = plant_sys.params.scale; animation_dt = 0.05; x_quan_vec = coordinateTransformation(xs); anim_flat_ground(ts, x_quan_vec, animation_dt) diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index a4fd6b2..8cfe77a 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -2,7 +2,7 @@ % Aaron Ames et al. Control Barrier Function based Quadratic Programs % with Application to Adaptive Cruise Control, CDC 2014, Table 1. -classdef RabbitBuiltIn < CtrlAffineSysFL +classdef RabbitBuiltIn < CtrlAffineSysFL methods function obj = RabbitBuiltIn(params) % Always using built-in option for setup. @@ -21,54 +21,7 @@ % RABBIT g(x) [~, g_] = obj.defineSystem(x); end - function V = clf(obj, x) - % TODO: Lyapunov Function: redundant actually - n = obj.xdim/2; - y = obj.y(x); - dy = obj.lf_y(x); - eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; - P = obj.Gram_clf_FL; - % In case of sanity check - if isempty(obj.Gram_clf_FL) - P = eye(obj.udim * 2); % exploiting obj.udim = obj.ydim - end - V = transpose(eta_eps)*P*eta_eps; - end - function LfV = lf_clf(obj, x) - % TODO: LfV(x) : redundant => CtrlAffineSysFL rehandled - n = obj.xdim/2; - y = obj.y(x); - dy = obj.lf_y(x); - eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; - - P = obj.Gram_clf_FL; - F_FL_eps = obj.F_FL_eps; - - % In case of sanity check - if isempty(obj.F_FL_eps) - F_FL_eps = eye(2 * obj.udim); - P = eye(2 * obj.udim); - end - LfV = transpose(eta_eps)*(transpose(F_FL_eps)*P+P*F_FL_eps)*eta_eps; - end - function LgV = lg_clf(obj, x) - % TODO: LgV(x) : redundant => CtrlAffineSysFL rehandled - n = obj.xdim/2; - y = obj.y(x); - dy = obj.lf_y(x); - eta_eps = [(1/obj.params.epsilon_FL)*y; dy]; - - P = obj.Gram_clf_FL; - G_FL = obj.G_FL; - - if isempty(obj.G_FL) % In case of sanity check - G_FL = [zeros(obj.udim); - eye(obj.udim)]; - P = eye(2*obj.udim); - end - - LgV = (2 * (G_FL'*P) * eta_eps)'; - end + function B = cbf(obj, x) % TODO: CBF(x) end @@ -90,14 +43,14 @@ theta_end = obj.params.legacy.theta_end; phase = min(max((theta - theta_init)/(theta_end - theta_init))); - yd = bezier_outputs(a_bez, phase); % Not very structured actually + yd = obj.bezier_outputs(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 phase_ = phase(obj, x) +% % TODO: Rabbit's phase => redundant(?) +% end function Lfy = lf_y(obj, x) % TODO: Not used in RABBIT @@ -109,8 +62,8 @@ theta_init = obj.params.legacy.theta_init; theta_end = obj.params.legacy.theta_end; - dydq_des = dydq_d_gen(q, a_bez, theta_init, theta_end); %[4 x 7] - dydq_act = dydq_a_gen(q); + dydq_des = obj.dydq_d_gen(q, a_bez, theta_init, 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 @@ -129,8 +82,8 @@ theta_init = obj.params.legacy.theta_init; theta_end = obj.params.legacy.theta_end; - dydq_des = dydq_d_gen(q, a_bez, theta_init, theta_end); %[4 x 7] - dydq_act = dydq_a_gen(q); + dydq_des = obj.dydq_d_gen(q, a_bez, theta_init, 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, :); @@ -146,42 +99,568 @@ theta_init = obj.params.legacy.theta_init; theta_end = obj.params.legacy.theta_end; - L2ydq_des = L2ydq_d_gen(q, dq, a_bez, theta_init, theta_end); - L2ydq_act = L2ydq_a_gen(q, dq); + L2ydq_des = obj.L2ydq_d_gen(q, dq, a_bez, theta_init, theta_end); + L2ydq_act = obj.L2ydq_a_gen(q, dq); f_s = obj.f(x); L2fy = (L2ydq_act - L2ydq_des)*f_s; end - function y_max_exceed_ = y_max_exceed(obj, x) - end - - function lf_y_max_exceed_ = lf_y_max_exceed(obj, x) - end - - function lg_y_max_exceed_ = lg_y_max_exceed(obj, x) - end - - function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) + %% Generator + 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 + 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 - - function l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) + 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 - - function y_min_exceed_ = y_min_exceed(obj, x) + 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 - - function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) + 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 - - function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) + 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 - - function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) + 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 - - function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) + 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 +% function y_max_exceed_ = y_max_exceed(obj, x) +% end +% +% function lf_y_max_exceed_ = lf_y_max_exceed(obj, x) +% end +% +% function lg_y_max_exceed_ = lg_y_max_exceed(obj, x) +% end +% +% function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) +% end +% +% function l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) +% end +% +% function y_min_exceed_ = y_min_exceed(obj, x) +% end +% +% function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) +% end +% +% function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) +% end +% +% function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) +% end +% +% function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) +% 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/defineSystem.m b/dynsys/@RabbitBuiltIn/defineSystem.m index 181a2a4..2926de8 100644 --- a/dynsys/@RabbitBuiltIn/defineSystem.m +++ b/dynsys/@RabbitBuiltIn/defineSystem.m @@ -11,10 +11,12 @@ q = x(1:n); dq = x(n+1:2*n); - Dm = D_gen_rel(q,1); % 7 x 7 - Cm = C_gen_rel(q,dq,1); % 7 x 7 - Gm = G_gen_rel(q,1); - Bm =[0 0 0 0 + %[D, C, G, B] = obj.gen_DCGB_rel(q); + %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 + 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 @@ -26,17 +28,17 @@ dJSt = dJ_RightToe(q,dq); dJSt = dJSt([1,3],:); - Hm= Cm*dq + Gm; + H = C*dq + G; % Constraint Force to enforce the holonomic constraint: - FSt_u_m = - pinv(JSt*(Dm\JSt'))*(JSt*(Dm\Bm)); - FSt_nu_m = - pinv(JSt*(Dm\JSt'))*(JSt*(Dm\(-Hm)) + dJSt*dq); + FSt_u = - pinv(JSt*(D\JSt'))*(JSt*(D\B)); + FSt_nu = - pinv(JSt*(D\JSt'))*(JSt*(D\(-H)) + dJSt*dq); - f_m = [dq; Dm\(-Cm*dq - Gm)]; - g1_m = [zeros(length(dq),obj.udim); Dm\Bm]; - g2_m = [zeros(length(dq),size(FSt_nu_m,1)); Dm\JSt']; + 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_m + g2_m*(FSt_nu_m); - g = g1_m + g2_m*FSt_u_m; + f = f + g2 * FSt_nu; + g = g1 + g2 * FSt_u; end 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/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 80f1227..333621d 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -109,15 +109,14 @@ 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); catch e n_clf = 0; end obj.n_clf = n_clf; n_cbf = 1; try - obj.cbf(x_test); - disp(obj.cbf(x_test)); + cbf_test = obj.cbf(x_test); catch e n_cbf = 0; end diff --git a/utils/rollout_feedback_linearization.m b/utils/rollout_feedback_linearization.m index f0634e7..44a2d0e 100644 --- a/utils/rollout_feedback_linearization.m +++ b/utils/rollout_feedback_linearization.m @@ -1,5 +1,5 @@ function [xs, us, ts, Vs, dVs_error, ys] = rollout_feedback_linearization( ... - x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_func) + x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_options) % controller = CBF-CLF-QP if nargin < 7 with_slack = 0; @@ -17,7 +17,7 @@ end if nargin < 10 - event_func = []; % 0: no event function + event_options = []; % 0: no event function end % Total time step @@ -63,11 +63,10 @@ us(k, :) = u; % Run simulation for one time step with determined control input. - if isempty(event_func) + if isempty(event_options) [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! else - options = odeset('Events', event_func); - [ts_k, xs_k, te, ~, ~] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, options); %TODO => eventfunc!! + [ts_k, xs_k, te, ~, ~] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! end t = ts_k(end); From d9fb5ab12782d252142074726f2d05fafa0bac63 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Thu, 12 Aug 2021 12:26:00 +0900 Subject: [PATCH 10/49] usage tutorial added --- readme_wonsuhk.txt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 readme_wonsuhk.txt diff --git a/readme_wonsuhk.txt b/readme_wonsuhk.txt new file mode 100644 index 0000000..9cb06cd --- /dev/null +++ b/readme_wonsuhk.txt @@ -0,0 +1,4 @@ +In order to tune the scale parameter of the plant system, +tune the plant_sys.params.scale in main script + +run_clf_simulation_rabbit.m > line 40 \ No newline at end of file From f0e71348bb3c0e5da12f6f6ac8a36b949cae61ad Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Thu, 12 Aug 2021 16:44:11 +0900 Subject: [PATCH 11/49] 1. Deleted redundant directory 2. Saved functions required to plot the rabbit under utils\plot_helper 3. Simulation Result visualization completed --- backup.txt | 86 ------- demos/run_clf_simulation_rabbit.m | 70 +++++- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 18 +- dynsys/@RabbitBuiltIn/get_force.m | 32 +++ .../{FROST => }/Ce_five_link_walker.mexw64 | Bin .../{FROST => }/De_five_link_walker.mexw64 | Bin .../{FROST => }/Ge_five_link_walker.mexw64 | Bin .../{FROST => }/J_LeftToe.mexw64 | Bin .../{FROST => }/J_RightToe.mexw64 | Bin dynsys_helper/Rabbit_helper/bezier_outputs.m | 25 -- .../{FROST => }/dJ_RightToe.mexw64 | Bin dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m | 8 - dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m | 192 --------------- dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m | 23 -- .../Rabbit_helper/gen/dq_pos_gen_v2.m | 21 -- dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m | 8 - dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m | 77 ------ dynsys_helper/Rabbit_helper/gen/gradient_h1.m | 38 --- dynsys_helper/Rabbit_helper/gen/gradient_h2.m | 38 --- dynsys_helper/Rabbit_helper/gen/hessian_h1.m | 99 -------- dynsys_helper/Rabbit_helper/gen/hessian_h2.m | 99 -------- dynsys_helper/Rabbit_helper/gen/psi_gen.m | 11 - dynsys_helper/Rabbit_helper/modelParameters.m | 34 --- .../{FROST => }/p_LeftToe.mexw64 | Bin .../{FROST => }/p_RightToe.mexw64 | Bin .../Rabbit_helper/{FROST => }/p_Torso.mexw64 | Bin .../{FROST => }/p_q2_left.mexw64 | Bin .../{FROST => }/p_q2_right.mexw64 | Bin .../{FROST => }/trajectory_data.mat | Bin dynsys_helper/readme_rabbit.txt | 27 +++ readme_wonsuhk.txt | 4 - us_Vs.mat | Bin 329 -> 0 bytes utils/plot_helper/callPlotSettings.m | 7 + utils/plot_helper/subplot1.m | 228 ++++++++++++++++++ utils/rollout_feedback_linearization.m | 11 +- 35 files changed, 388 insertions(+), 768 deletions(-) delete mode 100644 backup.txt create mode 100644 dynsys/@RabbitBuiltIn/get_force.m rename dynsys_helper/Rabbit_helper/{FROST => }/Ce_five_link_walker.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/De_five_link_walker.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/Ge_five_link_walker.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/J_LeftToe.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/J_RightToe.mexw64 (100%) delete mode 100644 dynsys_helper/Rabbit_helper/bezier_outputs.m rename dynsys_helper/Rabbit_helper/{FROST => }/dJ_RightToe.mexw64 (100%) delete mode 100644 dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/dq_pos_gen_v2.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/gradient_h1.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/gradient_h2.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/hessian_h1.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/hessian_h2.m delete mode 100644 dynsys_helper/Rabbit_helper/gen/psi_gen.m delete mode 100644 dynsys_helper/Rabbit_helper/modelParameters.m rename dynsys_helper/Rabbit_helper/{FROST => }/p_LeftToe.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/p_RightToe.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/p_Torso.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/p_q2_left.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/p_q2_right.mexw64 (100%) rename dynsys_helper/Rabbit_helper/{FROST => }/trajectory_data.mat (100%) create mode 100644 dynsys_helper/readme_rabbit.txt delete mode 100644 readme_wonsuhk.txt delete mode 100644 us_Vs.mat create mode 100644 utils/plot_helper/callPlotSettings.m create mode 100644 utils/plot_helper/subplot1.m diff --git a/backup.txt b/backup.txt deleted file mode 100644 index ce1f409..0000000 --- a/backup.txt +++ /dev/null @@ -1,86 +0,0 @@ -for k = 1:total_k-1 - t - % Determine control input. - % dV_hat: analytic Vdot based on model. - [u, slack, V] = controller(x); -% [u, slack, h, V] = controller(s, u_prev); % optimizing the difference between the previous timestep. - 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); - u_prev = u; - t = t + dt; -end - -plot_results(ts, xs, us, params.p_o, params.r_o) - -%% Auxilary Functions -function plot_results(t, xs, us, p_o, r_o) - -figure -subplot(5,1,1) -plot(t, xs(:,1)) -xlabel('t') -ylabel('x [m]') - -subplot(5,1,2) -plot(t, xs(:,2)) -xlabel('t') -ylabel('v_x [m/s]') - -subplot(5,1,3) -plot(t, xs(:,3)) -xlabel('t') -ylabel('y [m]') - -subplot(5,1,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)) -xlabel('t') -ylabel('v [m/s]') - - -figure -subplot(2,1,1) -plot(t(1:end-1), us(:,1)) -xlabel('t') -ylabel('a_x [m/s^2]') - -subplot(2,1,2) -plot(t(1:end-1), 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([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)); -draw_circle(p_o, r_o); - -xlim([lim_min, lim_max]); -ylim([lim_min, lim_max]); -xlabel('x [m]') -ylabel('y [m]') -end - -function h = draw_circle(center,r) -hold on -th = 0:pi/50:2*pi; -xunit = r * cos(th) + center(1); -yunit = r * sin(th) + center(2); -h = plot(xunit, yunit); -hold off -end \ No newline at end of file diff --git a/demos/run_clf_simulation_rabbit.m b/demos/run_clf_simulation_rabbit.m index 62ab87c..ed92f98 100644 --- a/demos/run_clf_simulation_rabbit.m +++ b/demos/run_clf_simulation_rabbit.m @@ -4,6 +4,7 @@ close all; clear all; clc; %% Animation setting global animation_scale % animation scale declared global + %% Set Parameters % Simulation Setting % sim_t: overall simulation time: do not need it in this mode actually @@ -14,7 +15,7 @@ % with slack: slack variable in QP(1), no slack variable in QP(0) params = init_clf_simulation_rabbit; % Load all the parameters -dt = 0.025; % simulation interval +dt = 0.01; % simulation interval sim_t = 100; % simulation time nstep = 10; % number of steps with_slack = true; % use slack variable in QP (change this into false in order not to use it) @@ -37,9 +38,10 @@ %% Prepare System control_sys = RabbitBuiltIn(params); plant_sys = RabbitBuiltIn(params); -plant_sys.params.scale = 2.5; % Reflect model uncertainty here +plant_sys.params.scale = 3; % Reflect model uncertainty here +%plant_sys.params.torso_add = 10; controller = @control_sys.ctrlClfQpFL; -event_options = odeset('Events', @control_sys.rabbit_event); +event_options = odeset('Events', @plant_sys.rabbit_event); %odeFun = @plant_sys.dynamics; %odeSolver = @ode45; @@ -79,7 +81,69 @@ ts = [ts; ts_new(2:end)]; xs = [xs; xs_new(2:end, :)]; end +%% New Figures +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(2:end), plant_sys.y_out_history(:,1)); grid on; +xlabel('Time(s)'); ylabel('q_1'); +title('Stance Leg Hip Angle'); + +figure(2); +plot(ts(2:end),plant_sys.y_out_history(:,2)); grid on; +xlabel('Time(s)'); ylabel('q_3'); +title('Stance Leg Knee Angle'); + +figure(3); +plot(ts(2:end),plant_sys.y_out_history(:,3)); grid on; +xlabel('Time(s)'); ylabel('q_2'); +title('Swing Leg Hip Angle'); + +figure(4); +plot(ts(2:end),plant_sys.y_out_history(:,4)); grid on; +xlabel('Time(s)'); ylabel('q_4'); +title('Swing Leg Knee Angle'); + +figure(5); plot(ts(2:end), plant_sys.F_st_history(:,1)); grid on; hold on; xlabel('t'); +ylabel('FSt'); plot(ts(2:end), plant_sys.F_st_history(:,2)); legend(['FSt(1)';'FSt(2)']); + +figure(6); +plot(ts(2:end),abs(plant_sys.F_st_history(:,1)./plant_sys.F_st_history(:,2)), 'color', main_color_blue, 'LineWidth', 1); grid on; hold on; +title('Contact Force Ratio ($|F_{T}$/$F_{N}|$)', 'Interpreter', 'latex'); +xlim([0 ts(end)+1]); +xlabel('Time (sec)'); +ylabel('$|F_{T}$/$F_{N}|$', 'Interpreter', 'latex'); +callPlotSettings(fig_sz, plot_pos); +%print('-f6','plot_F_ratio', '-dpng'); + +figure(7); +plot(ts(2:end),plant_sys.u_motor_history); grid on; hold on; +title('torques'); legend('u1','u2','u3','u4'); + +figure(9); +subplot1(2,1,'Gap', subplot_gap); +subplot1(1); hold on; +title('Euclidian Norm of the Tracking Error'); +plot(ts(2:end),sqrt(plant_sys.y_out_history(:,1).^2+plant_sys.y_out_history(:,2).^2+... + plant_sys.y_out_history(:,3).^2+plant_sys.y_out_history(:,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(2:end),sqrt(plant_sys.dy_out_history(:,1).^2+plant_sys.dy_out_history(:,2).^2+... + plant_sys.dy_out_history(:,3).^2+plant_sys.dy_out_history(:,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'); %% Five Link Animation animation_scale = plant_sys.params.scale; animation_dt = 0.05; diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index 8cfe77a..16f98d8 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -3,10 +3,27 @@ % with Application to Adaptive Cruise Control, CDC 2014, Table 1. classdef RabbitBuiltIn < CtrlAffineSysFL + properties + F_st_history % stance foot horizontal force + y_out_history % output history + dy_out_history % output derivative history + u_motor_history % motor input history + end methods function obj = RabbitBuiltIn(params) % Always using built-in option for setup. obj = obj@CtrlAffineSysFL(params, 'built-in'); + obj.F_st_history = []; + obj.y_out_history = []; + obj.dy_out_history = []; + obj.u_motor_history = []; + end + + function clear_properties(obj) + obj.F_st_history = []; + obj.y_out_history = []; + obj.dy_out_history = []; + obj.u_motor_history = []; end function f_ = f(obj, x) % RABBIT f(x) @@ -21,7 +38,6 @@ % RABBIT g(x) [~, g_] = obj.defineSystem(x); end - function B = cbf(obj, x) % TODO: CBF(x) end diff --git a/dynsys/@RabbitBuiltIn/get_force.m b/dynsys/@RabbitBuiltIn/get_force.m new file mode 100644 index 0000000..80e7d5e --- /dev/null +++ b/dynsys/@RabbitBuiltIn/get_force.m @@ -0,0 +1,32 @@ +function [FSt_u, FSt_nu] = get_force(obj, x) + n = obj.xdim / 2; + if obj.params.mode.motor_dynamics + u_s = x(end-3 : end); + end + + q = x(1:n); + dq = x(n+1:2*n); + + %[D, C, G, B] = obj.gen_DCGB_rel(q); + %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 + 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]; + 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); +end + diff --git a/dynsys_helper/Rabbit_helper/FROST/Ce_five_link_walker.mexw64 b/dynsys_helper/Rabbit_helper/Ce_five_link_walker.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/Ce_five_link_walker.mexw64 rename to dynsys_helper/Rabbit_helper/Ce_five_link_walker.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/De_five_link_walker.mexw64 b/dynsys_helper/Rabbit_helper/De_five_link_walker.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/De_five_link_walker.mexw64 rename to dynsys_helper/Rabbit_helper/De_five_link_walker.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/Ge_five_link_walker.mexw64 b/dynsys_helper/Rabbit_helper/Ge_five_link_walker.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/Ge_five_link_walker.mexw64 rename to dynsys_helper/Rabbit_helper/Ge_five_link_walker.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/J_LeftToe.mexw64 b/dynsys_helper/Rabbit_helper/J_LeftToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/J_LeftToe.mexw64 rename to dynsys_helper/Rabbit_helper/J_LeftToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/J_RightToe.mexw64 b/dynsys_helper/Rabbit_helper/J_RightToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/J_RightToe.mexw64 rename to dynsys_helper/Rabbit_helper/J_RightToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/bezier_outputs.m b/dynsys_helper/Rabbit_helper/bezier_outputs.m deleted file mode 100644 index 436d211..0000000 --- a/dynsys_helper/Rabbit_helper/bezier_outputs.m +++ /dev/null @@ -1,25 +0,0 @@ -function fcn = bezier_outputs(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_helper/Rabbit_helper/FROST/dJ_RightToe.mexw64 b/dynsys_helper/Rabbit_helper/dJ_RightToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/dJ_RightToe.mexw64 rename to dynsys_helper/Rabbit_helper/dJ_RightToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m b/dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m deleted file mode 100644 index 4696613..0000000 --- a/dynsys_helper/Rabbit_helper/gen/L2ydq_a_gen.m +++ /dev/null @@ -1,8 +0,0 @@ -function L2ydq_a = L2ydq_a_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m b/dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m deleted file mode 100644 index 8e90b63..0000000 --- a/dynsys_helper/Rabbit_helper/gen/L2ydq_d_gen.m +++ /dev/null @@ -1,192 +0,0 @@ -function L2ydq_d = L2ydq_d_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m b/dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m deleted file mode 100644 index be8a6c6..0000000 --- a/dynsys_helper/Rabbit_helper/gen/dq_pos_gen.m +++ /dev/null @@ -1,23 +0,0 @@ -function dq_pos = dq_pos_gen(x) - -n = 7; - -dq_pre = x(n+1:end)'; -q_pre = x(1:n)'; - -% D = 6*De_five_link_walker(q_pre); -D = 1.2*De_five_link_walker(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_helper/Rabbit_helper/gen/dq_pos_gen_v2.m b/dynsys_helper/Rabbit_helper/gen/dq_pos_gen_v2.m deleted file mode 100644 index d450bb0..0000000 --- a/dynsys_helper/Rabbit_helper/gen/dq_pos_gen_v2.m +++ /dev/null @@ -1,21 +0,0 @@ -function dq_pos = dq_pos_gen_v2(x, scale) - -n = 7; - -dq_pre = x(n+1:end)'; -q_pre = x(1:n)'; - -D = D_gen_rel(q_pre,scale); -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_helper/Rabbit_helper/gen/dydq_a_gen.m b/dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m deleted file mode 100644 index c0628c4..0000000 --- a/dynsys_helper/Rabbit_helper/gen/dydq_a_gen.m +++ /dev/null @@ -1,8 +0,0 @@ -function dydq_a = dydq_a_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m b/dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m deleted file mode 100644 index c73c70c..0000000 --- a/dynsys_helper/Rabbit_helper/gen/dydq_d_gen.m +++ /dev/null @@ -1,77 +0,0 @@ -function dydq_d = dydq_d_gen(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]); diff --git a/dynsys_helper/Rabbit_helper/gen/gradient_h1.m b/dynsys_helper/Rabbit_helper/gen/gradient_h1.m deleted file mode 100644 index f451c19..0000000 --- a/dynsys_helper/Rabbit_helper/gen/gradient_h1.m +++ /dev/null @@ -1,38 +0,0 @@ -function gradient_h1 = gradient_h1(in1,L3,L4,R1,l_max) -%GRADIENT_H1 -% GRADIENT_H1 = GRADIENT_H1(IN1,L3,L4,R1,L_MAX) - -% This function was generated by the Symbolic Math Toolbox version 8.4. -% 24-Jan-2020 08:14:27 - -q1 = in1(1,:); -q2 = in1(2,:); -q3 = in1(3,:); -q4 = in1(4,:); -t2 = cos(q1); -t3 = cos(q2); -t4 = cos(q3); -t5 = cos(q4); -t6 = sin(q1); -t7 = sin(q2); -t8 = sin(q3); -t9 = sin(q4); -t10 = L3.*t2; -t11 = L3.*t3; -t12 = L4.*t4; -t13 = L4.*t5; -t14 = L3.*t6; -t15 = L3.*t7; -t16 = L4.*t8; -t17 = L4.*t9; -t18 = -t11; -t19 = -t13; -t20 = -t15; -t21 = -t17; -t22 = t10+t12+t18+t19; -t23 = R1+t14+t16+t20+t21; -t24 = t22.^2; -t25 = t23.^2; -t26 = t24+t25; -t27 = 1.0./sqrt(t26); -gradient_h1 = [t27.*(t10.*t23.*2.0-t14.*t22.*2.0).*(-1.0./2.0);(t27.*(t11.*t23.*2.0-t15.*t22.*2.0))./2.0;t27.*(t12.*t23.*2.0-t16.*t22.*2.0).*(-1.0./2.0);(t27.*(t13.*t23.*2.0-t17.*t22.*2.0))./2.0;0.0]; diff --git a/dynsys_helper/Rabbit_helper/gen/gradient_h2.m b/dynsys_helper/Rabbit_helper/gen/gradient_h2.m deleted file mode 100644 index 6e064c6..0000000 --- a/dynsys_helper/Rabbit_helper/gen/gradient_h2.m +++ /dev/null @@ -1,38 +0,0 @@ -function gradient_h2 = gradient_h2(in1,L3,L4,R2,l_min) -%GRADIENT_H2 -% GRADIENT_H2 = GRADIENT_H2(IN1,L3,L4,R2,L_MIN) - -% This function was generated by the Symbolic Math Toolbox version 8.4. -% 24-Jan-2020 08:14:34 - -q1 = in1(1,:); -q2 = in1(2,:); -q3 = in1(3,:); -q4 = in1(4,:); -t2 = cos(q1); -t3 = cos(q2); -t4 = cos(q3); -t5 = cos(q4); -t6 = sin(q1); -t7 = sin(q2); -t8 = sin(q3); -t9 = sin(q4); -t10 = L3.*t2; -t11 = L3.*t3; -t12 = L4.*t4; -t13 = L4.*t5; -t14 = L3.*t6; -t15 = L3.*t7; -t16 = L4.*t8; -t17 = L4.*t9; -t18 = -t10; -t19 = -t12; -t20 = -t14; -t21 = -t16; -t22 = l_min+t15+t17+t20+t21; -t23 = R2+t11+t13+t18+t19; -t24 = t23.^2; -t25 = t22.^2; -t26 = t24+t25; -t27 = 1.0./sqrt(t26); -gradient_h2 = [t27.*(t10.*t22.*2.0-t14.*t23.*2.0).*(-1.0./2.0);(t27.*(t11.*t22.*2.0-t15.*t23.*2.0))./2.0;t27.*(t12.*t22.*2.0-t16.*t23.*2.0).*(-1.0./2.0);(t27.*(t13.*t22.*2.0-t17.*t23.*2.0))./2.0;0.0]; diff --git a/dynsys_helper/Rabbit_helper/gen/hessian_h1.m b/dynsys_helper/Rabbit_helper/gen/hessian_h1.m deleted file mode 100644 index 34de3f4..0000000 --- a/dynsys_helper/Rabbit_helper/gen/hessian_h1.m +++ /dev/null @@ -1,99 +0,0 @@ -function hessian_h1 = hessian_h1(in1,L3,L4,R1,l_max) -%HESSIAN_H1 -% HESSIAN_H1 = HESSIAN_H1(IN1,L3,L4,R1,L_MAX) - -% This function was generated by the Symbolic Math Toolbox version 8.4. -% 24-Jan-2020 08:14:34 - -q1 = in1(1,:); -q2 = in1(2,:); -q3 = in1(3,:); -q4 = in1(4,:); -t2 = cos(q1); -t3 = cos(q2); -t4 = cos(q3); -t5 = cos(q4); -t6 = sin(q1); -t7 = sin(q2); -t8 = sin(q3); -t9 = sin(q4); -t10 = L3.^2; -t11 = L4.^2; -t12 = L3.*t2; -t13 = L3.*t3; -t14 = L4.*t4; -t15 = L4.*t5; -t16 = L3.*t6; -t17 = L3.*t7; -t18 = L4.*t8; -t19 = L4.*t9; -t32 = t2.*t3.*t10.*2.0; -t33 = t4.*t5.*t11.*2.0; -t34 = t6.*t7.*t10.*2.0; -t35 = t8.*t9.*t11.*2.0; -t20 = -t13; -t21 = -t15; -t22 = -t17; -t23 = -t19; -t24 = t12.*t14.*2.0; -t25 = t12.*t15.*2.0; -t26 = t13.*t14.*2.0; -t27 = t13.*t15.*2.0; -t28 = t16.*t18.*2.0; -t29 = t16.*t19.*2.0; -t30 = t17.*t18.*2.0; -t31 = t17.*t19.*2.0; -t40 = t32+t34; -t41 = t33+t35; -t36 = t24+t28; -t37 = t25+t29; -t38 = t26+t30; -t39 = t27+t31; -t42 = t12+t14+t20+t21; -t43 = R1+t16+t18+t22+t23; -t44 = t42.^2; -t45 = t43.^2; -t46 = t16.*t42.*2.0; -t47 = t17.*t42.*2.0; -t48 = t18.*t42.*2.0; -t49 = t19.*t42.*2.0; -t50 = t12.*t43.*2.0; -t51 = t13.*t43.*2.0; -t52 = t14.*t43.*2.0; -t53 = t15.*t43.*2.0; -t54 = -t46; -t55 = -t47; -t56 = -t48; -t57 = -t49; -t58 = t44+t45; -t59 = 1.0./sqrt(t58); -t61 = t50+t54; -t62 = t51+t55; -t63 = t52+t56; -t64 = t53+t57; -t60 = t59.^3; -t65 = (t36.*t59)./2.0; -t66 = (t37.*t59)./2.0; -t67 = (t38.*t59)./2.0; -t68 = (t39.*t59)./2.0; -t71 = (t40.*t59)./2.0; -t72 = (t41.*t59)./2.0; -t69 = -t65; -t70 = -t68; -t73 = (t60.*(t46-t50).*(t47-t51))./4.0; -t74 = (t60.*(t46-t50).*(t48-t52))./4.0; -t75 = (t60.*(t46-t50).*(t49-t53))./4.0; -t76 = (t60.*(t47-t51).*(t48-t52))./4.0; -t77 = (t60.*(t47-t51).*(t49-t53))./4.0; -t78 = (t60.*(t48-t52).*(t49-t53))./4.0; -t79 = -t73; -t80 = -t75; -t81 = -t76; -t82 = -t78; -t83 = t69+t74; -t86 = t70+t77; -t84 = t66+t80; -t85 = t67+t81; -t87 = t71+t79; -t88 = t72+t82; -hessian_h1 = reshape([(t59.*(t12.*t42.*2.0+t16.*t43.*2.0-t2.^2.*t10.*2.0-t6.^2.*t10.*2.0))./2.0+(t60.*(t46-t50).^2)./4.0,t87,t83,t84,0.0,t87,t59.*(t13.*t42.*2.0+t17.*t43.*2.0+t3.^2.*t10.*2.0+t7.^2.*t10.*2.0).*(-1.0./2.0)+(t60.*(t47-t51).^2)./4.0,t85,t86,0.0,t83,t85,(t59.*(t14.*t42.*2.0+t18.*t43.*2.0-t4.^2.*t11.*2.0-t8.^2.*t11.*2.0))./2.0+(t60.*(t48-t52).^2)./4.0,t88,0.0,t84,t86,t88,t59.*(t15.*t42.*2.0+t19.*t43.*2.0+t5.^2.*t11.*2.0+t9.^2.*t11.*2.0).*(-1.0./2.0)+(t60.*(t49-t53).^2)./4.0,0.0,0.0,0.0,0.0,0.0,0.0],[5,5]); diff --git a/dynsys_helper/Rabbit_helper/gen/hessian_h2.m b/dynsys_helper/Rabbit_helper/gen/hessian_h2.m deleted file mode 100644 index 57b2a47..0000000 --- a/dynsys_helper/Rabbit_helper/gen/hessian_h2.m +++ /dev/null @@ -1,99 +0,0 @@ -function hessian_h2 = hessian_h2(in1,L3,L4,R2,l_min) -%HESSIAN_H2 -% HESSIAN_H2 = HESSIAN_H2(IN1,L3,L4,R2,L_MIN) - -% This function was generated by the Symbolic Math Toolbox version 8.4. -% 24-Jan-2020 08:14:41 - -q1 = in1(1,:); -q2 = in1(2,:); -q3 = in1(3,:); -q4 = in1(4,:); -t2 = cos(q1); -t3 = cos(q2); -t4 = cos(q3); -t5 = cos(q4); -t6 = sin(q1); -t7 = sin(q2); -t8 = sin(q3); -t9 = sin(q4); -t10 = L3.^2; -t11 = L4.^2; -t12 = L3.*t2; -t13 = L3.*t3; -t14 = L4.*t4; -t15 = L4.*t5; -t16 = L3.*t6; -t17 = L3.*t7; -t18 = L4.*t8; -t19 = L4.*t9; -t32 = t2.*t3.*t10.*2.0; -t33 = t4.*t5.*t11.*2.0; -t34 = t6.*t7.*t10.*2.0; -t35 = t8.*t9.*t11.*2.0; -t20 = -t12; -t21 = -t14; -t22 = -t16; -t23 = -t18; -t24 = t12.*t14.*2.0; -t25 = t12.*t15.*2.0; -t26 = t13.*t14.*2.0; -t27 = t13.*t15.*2.0; -t28 = t16.*t18.*2.0; -t29 = t16.*t19.*2.0; -t30 = t17.*t18.*2.0; -t31 = t17.*t19.*2.0; -t40 = t32+t34; -t41 = t33+t35; -t36 = t24+t28; -t37 = t25+t29; -t38 = t26+t30; -t39 = t27+t31; -t42 = l_min+t17+t19+t22+t23; -t43 = R2+t13+t15+t20+t21; -t44 = t43.^2; -t45 = t42.^2; -t46 = t16.*t43.*2.0; -t47 = t17.*t43.*2.0; -t48 = t18.*t43.*2.0; -t49 = t19.*t43.*2.0; -t50 = t12.*t42.*2.0; -t51 = t13.*t42.*2.0; -t52 = t14.*t42.*2.0; -t53 = t15.*t42.*2.0; -t54 = -t50; -t55 = -t51; -t56 = -t52; -t57 = -t53; -t58 = t44+t45; -t59 = 1.0./sqrt(t58); -t61 = t46+t54; -t62 = t47+t55; -t63 = t48+t56; -t64 = t49+t57; -t60 = t59.^3; -t65 = (t36.*t59)./2.0; -t66 = (t37.*t59)./2.0; -t67 = (t38.*t59)./2.0; -t68 = (t39.*t59)./2.0; -t71 = (t40.*t59)./2.0; -t72 = (t41.*t59)./2.0; -t69 = -t66; -t70 = -t67; -t73 = -t71; -t74 = -t72; -t75 = (t60.*t61.*t62)./4.0; -t76 = (t60.*t61.*t63)./4.0; -t77 = (t60.*t61.*t64)./4.0; -t78 = (t60.*t62.*t63)./4.0; -t79 = (t60.*t62.*t64)./4.0; -t80 = (t60.*t63.*t64)./4.0; -t81 = -t76; -t82 = -t79; -t84 = t69+t77; -t85 = t70+t78; -t87 = t73+t75; -t88 = t74+t80; -t83 = t65+t81; -t86 = t68+t82; -hessian_h2 = reshape([(t59.*(t12.*t43.*2.0+t16.*t42.*2.0+t2.^2.*t10.*2.0+t6.^2.*t10.*2.0))./2.0-(t60.*t61.^2)./4.0,t87,t83,t84,0.0,t87,t59.*(t13.*t43.*2.0+t17.*t42.*2.0-t3.^2.*t10.*2.0-t7.^2.*t10.*2.0).*(-1.0./2.0)-(t60.*t62.^2)./4.0,t85,t86,0.0,t83,t85,(t59.*(t14.*t43.*2.0+t18.*t42.*2.0+t4.^2.*t11.*2.0+t8.^2.*t11.*2.0))./2.0-(t60.*t63.^2)./4.0,t88,0.0,t84,t86,t88,t59.*(t15.*t43.*2.0+t19.*t42.*2.0-t5.^2.*t11.*2.0-t9.^2.*t11.*2.0).*(-1.0./2.0)-(t60.*t64.^2)./4.0,0.0,0.0,0.0,0.0,0.0,0.0],[5,5]); diff --git a/dynsys_helper/Rabbit_helper/gen/psi_gen.m b/dynsys_helper/Rabbit_helper/gen/psi_gen.m deleted file mode 100644 index 5559643..0000000 --- a/dynsys_helper/Rabbit_helper/gen/psi_gen.m +++ /dev/null @@ -1,11 +0,0 @@ -function val = psi_gen(x,y) -a = 0.9; -val = -sign(y).*abs(y).^a-sign(phi(x,y,a)).*abs(phi(x,y,a)).^(a/(2-a)); - -end - -function val = phi(x,y,a) - -val = x + 1/(2-a)*sign(y).*abs(y).^(2-a); - -end \ No newline at end of file diff --git a/dynsys_helper/Rabbit_helper/modelParameters.m b/dynsys_helper/Rabbit_helper/modelParameters.m deleted file mode 100644 index f126231..0000000 --- a/dynsys_helper/Rabbit_helper/modelParameters.m +++ /dev/null @@ -1,34 +0,0 @@ -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 scale - -scale = 1; % need to be fixed -% 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/dynsys_helper/Rabbit_helper/FROST/p_LeftToe.mexw64 b/dynsys_helper/Rabbit_helper/p_LeftToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/p_LeftToe.mexw64 rename to dynsys_helper/Rabbit_helper/p_LeftToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_RightToe.mexw64 b/dynsys_helper/Rabbit_helper/p_RightToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/p_RightToe.mexw64 rename to dynsys_helper/Rabbit_helper/p_RightToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_Torso.mexw64 b/dynsys_helper/Rabbit_helper/p_Torso.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/p_Torso.mexw64 rename to dynsys_helper/Rabbit_helper/p_Torso.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_q2_left.mexw64 b/dynsys_helper/Rabbit_helper/p_q2_left.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/p_q2_left.mexw64 rename to dynsys_helper/Rabbit_helper/p_q2_left.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/p_q2_right.mexw64 b/dynsys_helper/Rabbit_helper/p_q2_right.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/p_q2_right.mexw64 rename to dynsys_helper/Rabbit_helper/p_q2_right.mexw64 diff --git a/dynsys_helper/Rabbit_helper/FROST/trajectory_data.mat b/dynsys_helper/Rabbit_helper/trajectory_data.mat similarity index 100% rename from dynsys_helper/Rabbit_helper/FROST/trajectory_data.mat rename to dynsys_helper/Rabbit_helper/trajectory_data.mat diff --git a/dynsys_helper/readme_rabbit.txt b/dynsys_helper/readme_rabbit.txt new file mode 100644 index 0000000..c63a7cc --- /dev/null +++ b/dynsys_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/readme_wonsuhk.txt b/readme_wonsuhk.txt deleted file mode 100644 index 9cb06cd..0000000 --- a/readme_wonsuhk.txt +++ /dev/null @@ -1,4 +0,0 @@ -In order to tune the scale parameter of the plant system, -tune the plant_sys.params.scale in main script - -run_clf_simulation_rabbit.m > line 40 \ No newline at end of file diff --git a/us_Vs.mat b/us_Vs.mat deleted file mode 100644 index c319b1b592ee315681b81e34ff3f8e9dfcdbfebd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 329 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NSi^}l&jxdzrA1O{LC$nIBzTR8seQeFncUR__gUoqQufKm2 z(?5_om0)w;95>{12oQ0(sMz7Pc2), + 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 diff --git a/utils/rollout_feedback_linearization.m b/utils/rollout_feedback_linearization.m index 44a2d0e..cf427b7 100644 --- a/utils/rollout_feedback_linearization.m +++ b/utils/rollout_feedback_linearization.m @@ -75,7 +75,7 @@ % x_next = clip_theta(xs_k(end, :))'; xs(k+1, :) = x_next; - % Recording Section: Evaluate CLF related values after one time step. + %% Recording Section: Evaluate CLF related values after one time step. % Record dVs_error(k) = dV - dV_hat % Vs = V_next [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); @@ -93,6 +93,15 @@ % if abs(dV-dV_hat) > 50 % disp("debug"); % end +%% Under examination + [F_st_u_next, F_st_nu_next] = plant_sys.get_force(x_next); + F_st_next = F_st_u_next*u + F_st_nu_next; + + plant_sys.y_out_history = [plant_sys.y_out_history; y_next']; + plant_sys.dy_out_history = [plant_sys.dy_out_history; dy_next']; + plant_sys.F_st_history = [plant_sys.F_st_history; F_st_next']; + plant_sys.u_motor_history = [plant_sys.u_motor_history; u']; + if verbose mu' fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... From 782b53a79a861a28a2b46db1964337fd522044d7 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sat, 18 Sep 2021 21:35:46 +0900 Subject: [PATCH 12/49] 1. Resolved the varargin, varargout issue 2. Restructured rabbit_helper, plot, anim directory into demos/ 3. CLF-Helper rabbit version annotated more detailed. --- .../rabbit/anim}/anim_flat_ground.m | 0 .../rabbit/anim}/anim_moving_stone_load.m | 0 .../anim}/anim_moving_stone_load_compare.m | 0 {anim => demos/rabbit/anim}/bar_percent.m | 0 .../rabbit/anim}/coordinateTransformation.m | 0 {anim => demos/rabbit/anim}/even_sample.m | 0 .../rabbit/anim}/make_fig_contact_force.m | 0 .../rabbit/anim}/make_fig_step_length.m | 0 .../anim}/make_fig_stick_moving_stone_load.m | 0 {anim => demos/rabbit/anim}/make_plots.m | 0 {anim => demos/rabbit/anim}/make_plots_com.m | 0 {anim => demos/rabbit/anim}/make_plots_quan.m | 0 .../rabbit/anim}/make_plots_quan_save_figs.m | 0 {anim => demos/rabbit/anim}/make_stick_fig.m | 0 .../rabbit/anim}/plot_contact_force.m | 0 .../rabbit/anim}/plot_contact_force_data.m | 0 {anim => demos/rabbit/anim}/plot_foot_traj.m | 0 {anim => demos/rabbit/anim}/plot_stick_fig.m | 0 .../rabbit/anim}/plot_swing_foot_trail.m | 0 .../anim}/plot_swing_foot_trail_full_stick.m | 0 {anim => demos/rabbit/anim}/pt_circle.m | 0 {anim => demos/rabbit/anim}/pt_circle2.m | 0 {anim => demos/rabbit/anim}/pt_line.m | 0 {anim => demos/rabbit/anim}/pt_link.m | 0 {anim => demos/rabbit/anim}/pt_link2.m | 0 .../anim}/rabbit_model/COM_pos_vel_accel.m | 0 .../anim}/rabbit_model/LyapRiccatiGenerator.m | 0 .../rabbit/anim}/rabbit_model/README.txt | 0 {anim => demos/rabbit/anim}/rabbit_model/bb.m | 0 .../rabbit/anim}/rabbit_model/bezier_obj.m | 0 .../anim}/rabbit_model/bezier_obj_jwg.m | 0 .../rabbit/anim}/rabbit_model/bezier_opt.m | 0 .../anim}/rabbit_model/bezier_opt_jwg.m | 0 .../anim}/rabbit_model/center_of_mass.m | 0 .../rabbit_model/compute_contact_force.m | 0 .../rabbit/anim}/rabbit_model/compute_p1_p5.m | 0 .../rabbit_model/compute_transition_ctrl.m | 0 .../anim}/rabbit_model/controlParameters.m | 0 .../rabbit/anim}/rabbit_model/control_synth.m | 0 .../rabbit/anim}/rabbit_model/dyn_mod_abs.m | 0 .../anim}/rabbit_model/energy_compute.m | 0 .../rabbit/anim}/rabbit_model/fixlength.m | 0 .../rabbit/anim}/rabbit_model/gen_zd.m | 0 .../rabbit/anim}/rabbit_model/gen_zd_cc.m | 0 .../rabbit/anim}/rabbit_model/gen_zd_diff.m | 0 .../anim}/rabbit_model/gen_zd_diff_cc.m | 0 .../rabbit/anim}/rabbit_model/head_height.m | 0 .../rabbit/anim}/rabbit_model/hip_pos.m | 0 .../rabbit/anim}/rabbit_model/hip_vel.m | 0 .../anim}/rabbit_model/impact_abs_red.m | 0 .../rabbit/anim}/rabbit_model/initialize.m | 0 .../rabbit/anim}/rabbit_model/limb_position.m | 0 .../anim}/rabbit_model/load_all_tests.m | 0 .../rabbit/anim}/rabbit_model/main_zd.m | 0 .../anim}/rabbit_model/miperhr2mpersec.m | 0 .../anim}/rabbit_model/modelParameters.m | 0 .../anim}/rabbit_model/mpersec2miperhr.m | 0 .../rabbit/anim}/rabbit_model/myaa.m | 0 .../anim}/rabbit_model/plot_ctrl_param_sens.m | 0 .../rabbit_model/plot_ctrl_param_sens_cc.m | 0 .../rabbit/anim}/rabbit_model/poincare_main.m | 0 .../anim}/rabbit_model/poincare_zd_compare.m | 0 .../rabbit_model/poincare_zd_compare_cc.m | 0 .../anim}/rabbit_model/poincare_zd_main.m | 0 .../anim}/rabbit_model/poincare_zd_main_cc.m | 0 .../rabbit/anim}/rabbit_model/quiver_erw.m | 0 .../rabbit/anim}/rabbit_model/rm_gaps.m | 0 .../rabbit/anim}/rabbit_model/sigma.m | 0 .../rabbit/anim}/rabbit_model/sigma_vel.m | 0 .../rabbit/anim}/rabbit_model/sim_zd.m | 0 .../rabbit/anim}/rabbit_model/sim_zd_cc.m | 0 .../rabbit/anim}/rabbit_model/sim_zd_opt.m | 0 .../rabbit_model/start_of_step_ctrl_sens.m | 0 .../rabbit/anim}/rabbit_model/step_length.m | 0 .../anim}/rabbit_model/swing_foot_height.m | 0 .../anim}/rabbit_model/swing_foot_velocity.m | 0 .../rabbit/anim}/rabbit_model/symb_control.m | 0 .../rabbit/anim}/rabbit_model/symb_model.m | 0 .../anim}/rabbit_model/symb_model_red.m | 0 .../rabbit/anim}/rabbit_model/theta_obj.m | 0 .../rabbit/anim}/rabbit_model/tilefigs.m | 0 .../anim}/rabbit_model/total_energy_red.m | 0 .../rabbit/anim}/rabbit_model/vectfield.m | 0 {anim => demos/rabbit/anim}/rabbit_model/zd.m | 0 .../rabbit/anim}/rabbit_model/zd_cc.m | 0 .../rabbit_model/zd_ctrl_sens_plots_cc.m | 0 .../rabbit/anim}/rabbit_model/zd_diff.m | 0 .../rabbit/anim}/rabbit_model/zd_diff_cc.m | 0 .../rabbit/anim}/rabbit_model/zd_lift.m | 0 .../anim}/rabbit_model/zd_lift_special.m | 0 .../rabbit/anim}/rabbit_model/zd_model.mdl | 0 .../rabbit/anim}/rabbit_model/zd_project.m | 0 .../rabbit/anim}/rabbit_model/zd_project_cc.m | 0 .../{ => rabbit}/init_clf_simulation_rabbit.m | 8 +- demos/rabbit/plot_rabbit_result.m | 89 ++++++++++ .../Rabbit_helper/Ce_five_link_walker.mexw64 | Bin .../Rabbit_helper/De_five_link_walker.mexw64 | Bin .../Rabbit_helper/Ge_five_link_walker.mexw64 | Bin .../Rabbit_helper/J_LeftToe.mexw64 | Bin .../Rabbit_helper/J_RightToe.mexw64 | Bin .../Rabbit_helper/dJ_RightToe.mexw64 | Bin .../Rabbit_helper/p_LeftToe.mexw64 | Bin .../Rabbit_helper/p_RightToe.mexw64 | Bin .../Rabbit_helper/p_Torso.mexw64 | Bin .../Rabbit_helper/p_q2_left.mexw64 | Bin .../Rabbit_helper/p_q2_right.mexw64 | Bin .../Rabbit_helper/trajectory_data.mat | Bin .../rabbit/rabbit_helper}/readme_rabbit.txt | 0 demos/rabbit/run_clf_simulation_rabbit.m | 92 ++++++++++ demos/run_clf_simulation_rabbit.m | 151 ---------------- demos/two_link_simulate.m | 155 ---------------- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 56 +++++- dynsys/@RabbitBuiltIn/defineSystem.m | 44 ----- lib/@CtrlAffineSys/CtrlAffineSys.m | 7 +- lib/@CtrlAffineSys/init_sys.m | 2 + lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 1 + lib/@CtrlAffineSysFL/ctrlClfQpFL.m | 6 +- lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m | 9 +- lib/@CtrlAffineSysFL/init_sys_FL.m | 3 + .../rollout_controller_eval_clf_FL.m | 168 ++++++++++++++++++ utils/rollout_feedback_linearization.m | 121 ------------- 121 files changed, 429 insertions(+), 483 deletions(-) rename {anim => demos/rabbit/anim}/anim_flat_ground.m (100%) rename {anim => demos/rabbit/anim}/anim_moving_stone_load.m (100%) rename {anim => demos/rabbit/anim}/anim_moving_stone_load_compare.m (100%) rename {anim => demos/rabbit/anim}/bar_percent.m (100%) rename {anim => demos/rabbit/anim}/coordinateTransformation.m (100%) rename {anim => demos/rabbit/anim}/even_sample.m (100%) rename {anim => demos/rabbit/anim}/make_fig_contact_force.m (100%) rename {anim => demos/rabbit/anim}/make_fig_step_length.m (100%) rename {anim => demos/rabbit/anim}/make_fig_stick_moving_stone_load.m (100%) rename {anim => demos/rabbit/anim}/make_plots.m (100%) rename {anim => demos/rabbit/anim}/make_plots_com.m (100%) rename {anim => demos/rabbit/anim}/make_plots_quan.m (100%) rename {anim => demos/rabbit/anim}/make_plots_quan_save_figs.m (100%) rename {anim => demos/rabbit/anim}/make_stick_fig.m (100%) rename {anim => demos/rabbit/anim}/plot_contact_force.m (100%) rename {anim => demos/rabbit/anim}/plot_contact_force_data.m (100%) rename {anim => demos/rabbit/anim}/plot_foot_traj.m (100%) rename {anim => demos/rabbit/anim}/plot_stick_fig.m (100%) rename {anim => demos/rabbit/anim}/plot_swing_foot_trail.m (100%) rename {anim => demos/rabbit/anim}/plot_swing_foot_trail_full_stick.m (100%) rename {anim => demos/rabbit/anim}/pt_circle.m (100%) rename {anim => demos/rabbit/anim}/pt_circle2.m (100%) rename {anim => demos/rabbit/anim}/pt_line.m (100%) rename {anim => demos/rabbit/anim}/pt_link.m (100%) rename {anim => demos/rabbit/anim}/pt_link2.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/COM_pos_vel_accel.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/LyapRiccatiGenerator.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/README.txt (100%) rename {anim => demos/rabbit/anim}/rabbit_model/bb.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/bezier_obj.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/bezier_obj_jwg.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/bezier_opt.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/bezier_opt_jwg.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/center_of_mass.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/compute_contact_force.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/compute_p1_p5.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/compute_transition_ctrl.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/controlParameters.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/control_synth.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/dyn_mod_abs.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/energy_compute.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/fixlength.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/gen_zd.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/gen_zd_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/gen_zd_diff.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/gen_zd_diff_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/head_height.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/hip_pos.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/hip_vel.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/impact_abs_red.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/initialize.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/limb_position.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/load_all_tests.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/main_zd.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/miperhr2mpersec.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/modelParameters.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/mpersec2miperhr.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/myaa.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/plot_ctrl_param_sens.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/plot_ctrl_param_sens_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/poincare_main.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/poincare_zd_compare.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/poincare_zd_compare_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/poincare_zd_main.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/poincare_zd_main_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/quiver_erw.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/rm_gaps.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/sigma.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/sigma_vel.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/sim_zd.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/sim_zd_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/sim_zd_opt.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/start_of_step_ctrl_sens.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/step_length.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/swing_foot_height.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/swing_foot_velocity.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/symb_control.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/symb_model.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/symb_model_red.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/theta_obj.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/tilefigs.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/total_energy_red.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/vectfield.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_ctrl_sens_plots_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_diff.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_diff_cc.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_lift.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_lift_special.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_model.mdl (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_project.m (100%) rename {anim => demos/rabbit/anim}/rabbit_model/zd_project_cc.m (100%) rename demos/{ => rabbit}/init_clf_simulation_rabbit.m (90%) create mode 100644 demos/rabbit/plot_rabbit_result.m rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/Ce_five_link_walker.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/De_five_link_walker.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/Ge_five_link_walker.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/J_LeftToe.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/J_RightToe.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/dJ_RightToe.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/p_LeftToe.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/p_RightToe.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/p_Torso.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/p_q2_left.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/p_q2_right.mexw64 (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/Rabbit_helper/trajectory_data.mat (100%) rename {dynsys_helper => demos/rabbit/rabbit_helper}/readme_rabbit.txt (100%) create mode 100644 demos/rabbit/run_clf_simulation_rabbit.m delete mode 100644 demos/run_clf_simulation_rabbit.m delete mode 100644 demos/two_link_simulate.m delete mode 100644 dynsys/@RabbitBuiltIn/defineSystem.m create mode 100644 lib/@CtrlAffineSysFL/rollout_controller_eval_clf_FL.m delete mode 100644 utils/rollout_feedback_linearization.m diff --git a/anim/anim_flat_ground.m b/demos/rabbit/anim/anim_flat_ground.m similarity index 100% rename from anim/anim_flat_ground.m rename to demos/rabbit/anim/anim_flat_ground.m diff --git a/anim/anim_moving_stone_load.m b/demos/rabbit/anim/anim_moving_stone_load.m similarity index 100% rename from anim/anim_moving_stone_load.m rename to demos/rabbit/anim/anim_moving_stone_load.m diff --git a/anim/anim_moving_stone_load_compare.m b/demos/rabbit/anim/anim_moving_stone_load_compare.m similarity index 100% rename from anim/anim_moving_stone_load_compare.m rename to demos/rabbit/anim/anim_moving_stone_load_compare.m diff --git a/anim/bar_percent.m b/demos/rabbit/anim/bar_percent.m similarity index 100% rename from anim/bar_percent.m rename to demos/rabbit/anim/bar_percent.m diff --git a/anim/coordinateTransformation.m b/demos/rabbit/anim/coordinateTransformation.m similarity index 100% rename from anim/coordinateTransformation.m rename to demos/rabbit/anim/coordinateTransformation.m diff --git a/anim/even_sample.m b/demos/rabbit/anim/even_sample.m similarity index 100% rename from anim/even_sample.m rename to demos/rabbit/anim/even_sample.m diff --git a/anim/make_fig_contact_force.m b/demos/rabbit/anim/make_fig_contact_force.m similarity index 100% rename from anim/make_fig_contact_force.m rename to demos/rabbit/anim/make_fig_contact_force.m diff --git a/anim/make_fig_step_length.m b/demos/rabbit/anim/make_fig_step_length.m similarity index 100% rename from anim/make_fig_step_length.m rename to demos/rabbit/anim/make_fig_step_length.m diff --git a/anim/make_fig_stick_moving_stone_load.m b/demos/rabbit/anim/make_fig_stick_moving_stone_load.m similarity index 100% rename from anim/make_fig_stick_moving_stone_load.m rename to demos/rabbit/anim/make_fig_stick_moving_stone_load.m diff --git a/anim/make_plots.m b/demos/rabbit/anim/make_plots.m similarity index 100% rename from anim/make_plots.m rename to demos/rabbit/anim/make_plots.m diff --git a/anim/make_plots_com.m b/demos/rabbit/anim/make_plots_com.m similarity index 100% rename from anim/make_plots_com.m rename to demos/rabbit/anim/make_plots_com.m diff --git a/anim/make_plots_quan.m b/demos/rabbit/anim/make_plots_quan.m similarity index 100% rename from anim/make_plots_quan.m rename to demos/rabbit/anim/make_plots_quan.m diff --git a/anim/make_plots_quan_save_figs.m b/demos/rabbit/anim/make_plots_quan_save_figs.m similarity index 100% rename from anim/make_plots_quan_save_figs.m rename to demos/rabbit/anim/make_plots_quan_save_figs.m diff --git a/anim/make_stick_fig.m b/demos/rabbit/anim/make_stick_fig.m similarity index 100% rename from anim/make_stick_fig.m rename to demos/rabbit/anim/make_stick_fig.m diff --git a/anim/plot_contact_force.m b/demos/rabbit/anim/plot_contact_force.m similarity index 100% rename from anim/plot_contact_force.m rename to demos/rabbit/anim/plot_contact_force.m diff --git a/anim/plot_contact_force_data.m b/demos/rabbit/anim/plot_contact_force_data.m similarity index 100% rename from anim/plot_contact_force_data.m rename to demos/rabbit/anim/plot_contact_force_data.m diff --git a/anim/plot_foot_traj.m b/demos/rabbit/anim/plot_foot_traj.m similarity index 100% rename from anim/plot_foot_traj.m rename to demos/rabbit/anim/plot_foot_traj.m diff --git a/anim/plot_stick_fig.m b/demos/rabbit/anim/plot_stick_fig.m similarity index 100% rename from anim/plot_stick_fig.m rename to demos/rabbit/anim/plot_stick_fig.m diff --git a/anim/plot_swing_foot_trail.m b/demos/rabbit/anim/plot_swing_foot_trail.m similarity index 100% rename from anim/plot_swing_foot_trail.m rename to demos/rabbit/anim/plot_swing_foot_trail.m diff --git a/anim/plot_swing_foot_trail_full_stick.m b/demos/rabbit/anim/plot_swing_foot_trail_full_stick.m similarity index 100% rename from anim/plot_swing_foot_trail_full_stick.m rename to demos/rabbit/anim/plot_swing_foot_trail_full_stick.m diff --git a/anim/pt_circle.m b/demos/rabbit/anim/pt_circle.m similarity index 100% rename from anim/pt_circle.m rename to demos/rabbit/anim/pt_circle.m diff --git a/anim/pt_circle2.m b/demos/rabbit/anim/pt_circle2.m similarity index 100% rename from anim/pt_circle2.m rename to demos/rabbit/anim/pt_circle2.m diff --git a/anim/pt_line.m b/demos/rabbit/anim/pt_line.m similarity index 100% rename from anim/pt_line.m rename to demos/rabbit/anim/pt_line.m diff --git a/anim/pt_link.m b/demos/rabbit/anim/pt_link.m similarity index 100% rename from anim/pt_link.m rename to demos/rabbit/anim/pt_link.m diff --git a/anim/pt_link2.m b/demos/rabbit/anim/pt_link2.m similarity index 100% rename from anim/pt_link2.m rename to demos/rabbit/anim/pt_link2.m diff --git a/anim/rabbit_model/COM_pos_vel_accel.m b/demos/rabbit/anim/rabbit_model/COM_pos_vel_accel.m similarity index 100% rename from anim/rabbit_model/COM_pos_vel_accel.m rename to demos/rabbit/anim/rabbit_model/COM_pos_vel_accel.m diff --git a/anim/rabbit_model/LyapRiccatiGenerator.m b/demos/rabbit/anim/rabbit_model/LyapRiccatiGenerator.m similarity index 100% rename from anim/rabbit_model/LyapRiccatiGenerator.m rename to demos/rabbit/anim/rabbit_model/LyapRiccatiGenerator.m diff --git a/anim/rabbit_model/README.txt b/demos/rabbit/anim/rabbit_model/README.txt similarity index 100% rename from anim/rabbit_model/README.txt rename to demos/rabbit/anim/rabbit_model/README.txt diff --git a/anim/rabbit_model/bb.m b/demos/rabbit/anim/rabbit_model/bb.m similarity index 100% rename from anim/rabbit_model/bb.m rename to demos/rabbit/anim/rabbit_model/bb.m diff --git a/anim/rabbit_model/bezier_obj.m b/demos/rabbit/anim/rabbit_model/bezier_obj.m similarity index 100% rename from anim/rabbit_model/bezier_obj.m rename to demos/rabbit/anim/rabbit_model/bezier_obj.m diff --git a/anim/rabbit_model/bezier_obj_jwg.m b/demos/rabbit/anim/rabbit_model/bezier_obj_jwg.m similarity index 100% rename from anim/rabbit_model/bezier_obj_jwg.m rename to demos/rabbit/anim/rabbit_model/bezier_obj_jwg.m diff --git a/anim/rabbit_model/bezier_opt.m b/demos/rabbit/anim/rabbit_model/bezier_opt.m similarity index 100% rename from anim/rabbit_model/bezier_opt.m rename to demos/rabbit/anim/rabbit_model/bezier_opt.m diff --git a/anim/rabbit_model/bezier_opt_jwg.m b/demos/rabbit/anim/rabbit_model/bezier_opt_jwg.m similarity index 100% rename from anim/rabbit_model/bezier_opt_jwg.m rename to demos/rabbit/anim/rabbit_model/bezier_opt_jwg.m diff --git a/anim/rabbit_model/center_of_mass.m b/demos/rabbit/anim/rabbit_model/center_of_mass.m similarity index 100% rename from anim/rabbit_model/center_of_mass.m rename to demos/rabbit/anim/rabbit_model/center_of_mass.m diff --git a/anim/rabbit_model/compute_contact_force.m b/demos/rabbit/anim/rabbit_model/compute_contact_force.m similarity index 100% rename from anim/rabbit_model/compute_contact_force.m rename to demos/rabbit/anim/rabbit_model/compute_contact_force.m diff --git a/anim/rabbit_model/compute_p1_p5.m b/demos/rabbit/anim/rabbit_model/compute_p1_p5.m similarity index 100% rename from anim/rabbit_model/compute_p1_p5.m rename to demos/rabbit/anim/rabbit_model/compute_p1_p5.m diff --git a/anim/rabbit_model/compute_transition_ctrl.m b/demos/rabbit/anim/rabbit_model/compute_transition_ctrl.m similarity index 100% rename from anim/rabbit_model/compute_transition_ctrl.m rename to demos/rabbit/anim/rabbit_model/compute_transition_ctrl.m diff --git a/anim/rabbit_model/controlParameters.m b/demos/rabbit/anim/rabbit_model/controlParameters.m similarity index 100% rename from anim/rabbit_model/controlParameters.m rename to demos/rabbit/anim/rabbit_model/controlParameters.m diff --git a/anim/rabbit_model/control_synth.m b/demos/rabbit/anim/rabbit_model/control_synth.m similarity index 100% rename from anim/rabbit_model/control_synth.m rename to demos/rabbit/anim/rabbit_model/control_synth.m diff --git a/anim/rabbit_model/dyn_mod_abs.m b/demos/rabbit/anim/rabbit_model/dyn_mod_abs.m similarity index 100% rename from anim/rabbit_model/dyn_mod_abs.m rename to demos/rabbit/anim/rabbit_model/dyn_mod_abs.m diff --git a/anim/rabbit_model/energy_compute.m b/demos/rabbit/anim/rabbit_model/energy_compute.m similarity index 100% rename from anim/rabbit_model/energy_compute.m rename to demos/rabbit/anim/rabbit_model/energy_compute.m diff --git a/anim/rabbit_model/fixlength.m b/demos/rabbit/anim/rabbit_model/fixlength.m similarity index 100% rename from anim/rabbit_model/fixlength.m rename to demos/rabbit/anim/rabbit_model/fixlength.m diff --git a/anim/rabbit_model/gen_zd.m b/demos/rabbit/anim/rabbit_model/gen_zd.m similarity index 100% rename from anim/rabbit_model/gen_zd.m rename to demos/rabbit/anim/rabbit_model/gen_zd.m diff --git a/anim/rabbit_model/gen_zd_cc.m b/demos/rabbit/anim/rabbit_model/gen_zd_cc.m similarity index 100% rename from anim/rabbit_model/gen_zd_cc.m rename to demos/rabbit/anim/rabbit_model/gen_zd_cc.m diff --git a/anim/rabbit_model/gen_zd_diff.m b/demos/rabbit/anim/rabbit_model/gen_zd_diff.m similarity index 100% rename from anim/rabbit_model/gen_zd_diff.m rename to demos/rabbit/anim/rabbit_model/gen_zd_diff.m diff --git a/anim/rabbit_model/gen_zd_diff_cc.m b/demos/rabbit/anim/rabbit_model/gen_zd_diff_cc.m similarity index 100% rename from anim/rabbit_model/gen_zd_diff_cc.m rename to demos/rabbit/anim/rabbit_model/gen_zd_diff_cc.m diff --git a/anim/rabbit_model/head_height.m b/demos/rabbit/anim/rabbit_model/head_height.m similarity index 100% rename from anim/rabbit_model/head_height.m rename to demos/rabbit/anim/rabbit_model/head_height.m diff --git a/anim/rabbit_model/hip_pos.m b/demos/rabbit/anim/rabbit_model/hip_pos.m similarity index 100% rename from anim/rabbit_model/hip_pos.m rename to demos/rabbit/anim/rabbit_model/hip_pos.m diff --git a/anim/rabbit_model/hip_vel.m b/demos/rabbit/anim/rabbit_model/hip_vel.m similarity index 100% rename from anim/rabbit_model/hip_vel.m rename to demos/rabbit/anim/rabbit_model/hip_vel.m diff --git a/anim/rabbit_model/impact_abs_red.m b/demos/rabbit/anim/rabbit_model/impact_abs_red.m similarity index 100% rename from anim/rabbit_model/impact_abs_red.m rename to demos/rabbit/anim/rabbit_model/impact_abs_red.m diff --git a/anim/rabbit_model/initialize.m b/demos/rabbit/anim/rabbit_model/initialize.m similarity index 100% rename from anim/rabbit_model/initialize.m rename to demos/rabbit/anim/rabbit_model/initialize.m diff --git a/anim/rabbit_model/limb_position.m b/demos/rabbit/anim/rabbit_model/limb_position.m similarity index 100% rename from anim/rabbit_model/limb_position.m rename to demos/rabbit/anim/rabbit_model/limb_position.m diff --git a/anim/rabbit_model/load_all_tests.m b/demos/rabbit/anim/rabbit_model/load_all_tests.m similarity index 100% rename from anim/rabbit_model/load_all_tests.m rename to demos/rabbit/anim/rabbit_model/load_all_tests.m diff --git a/anim/rabbit_model/main_zd.m b/demos/rabbit/anim/rabbit_model/main_zd.m similarity index 100% rename from anim/rabbit_model/main_zd.m rename to demos/rabbit/anim/rabbit_model/main_zd.m diff --git a/anim/rabbit_model/miperhr2mpersec.m b/demos/rabbit/anim/rabbit_model/miperhr2mpersec.m similarity index 100% rename from anim/rabbit_model/miperhr2mpersec.m rename to demos/rabbit/anim/rabbit_model/miperhr2mpersec.m diff --git a/anim/rabbit_model/modelParameters.m b/demos/rabbit/anim/rabbit_model/modelParameters.m similarity index 100% rename from anim/rabbit_model/modelParameters.m rename to demos/rabbit/anim/rabbit_model/modelParameters.m diff --git a/anim/rabbit_model/mpersec2miperhr.m b/demos/rabbit/anim/rabbit_model/mpersec2miperhr.m similarity index 100% rename from anim/rabbit_model/mpersec2miperhr.m rename to demos/rabbit/anim/rabbit_model/mpersec2miperhr.m diff --git a/anim/rabbit_model/myaa.m b/demos/rabbit/anim/rabbit_model/myaa.m similarity index 100% rename from anim/rabbit_model/myaa.m rename to demos/rabbit/anim/rabbit_model/myaa.m diff --git a/anim/rabbit_model/plot_ctrl_param_sens.m b/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens.m similarity index 100% rename from anim/rabbit_model/plot_ctrl_param_sens.m rename to demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens.m diff --git a/anim/rabbit_model/plot_ctrl_param_sens_cc.m b/demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens_cc.m similarity index 100% rename from anim/rabbit_model/plot_ctrl_param_sens_cc.m rename to demos/rabbit/anim/rabbit_model/plot_ctrl_param_sens_cc.m diff --git a/anim/rabbit_model/poincare_main.m b/demos/rabbit/anim/rabbit_model/poincare_main.m similarity index 100% rename from anim/rabbit_model/poincare_main.m rename to demos/rabbit/anim/rabbit_model/poincare_main.m diff --git a/anim/rabbit_model/poincare_zd_compare.m b/demos/rabbit/anim/rabbit_model/poincare_zd_compare.m similarity index 100% rename from anim/rabbit_model/poincare_zd_compare.m rename to demos/rabbit/anim/rabbit_model/poincare_zd_compare.m diff --git a/anim/rabbit_model/poincare_zd_compare_cc.m b/demos/rabbit/anim/rabbit_model/poincare_zd_compare_cc.m similarity index 100% rename from anim/rabbit_model/poincare_zd_compare_cc.m rename to demos/rabbit/anim/rabbit_model/poincare_zd_compare_cc.m diff --git a/anim/rabbit_model/poincare_zd_main.m b/demos/rabbit/anim/rabbit_model/poincare_zd_main.m similarity index 100% rename from anim/rabbit_model/poincare_zd_main.m rename to demos/rabbit/anim/rabbit_model/poincare_zd_main.m diff --git a/anim/rabbit_model/poincare_zd_main_cc.m b/demos/rabbit/anim/rabbit_model/poincare_zd_main_cc.m similarity index 100% rename from anim/rabbit_model/poincare_zd_main_cc.m rename to demos/rabbit/anim/rabbit_model/poincare_zd_main_cc.m diff --git a/anim/rabbit_model/quiver_erw.m b/demos/rabbit/anim/rabbit_model/quiver_erw.m similarity index 100% rename from anim/rabbit_model/quiver_erw.m rename to demos/rabbit/anim/rabbit_model/quiver_erw.m diff --git a/anim/rabbit_model/rm_gaps.m b/demos/rabbit/anim/rabbit_model/rm_gaps.m similarity index 100% rename from anim/rabbit_model/rm_gaps.m rename to demos/rabbit/anim/rabbit_model/rm_gaps.m diff --git a/anim/rabbit_model/sigma.m b/demos/rabbit/anim/rabbit_model/sigma.m similarity index 100% rename from anim/rabbit_model/sigma.m rename to demos/rabbit/anim/rabbit_model/sigma.m diff --git a/anim/rabbit_model/sigma_vel.m b/demos/rabbit/anim/rabbit_model/sigma_vel.m similarity index 100% rename from anim/rabbit_model/sigma_vel.m rename to demos/rabbit/anim/rabbit_model/sigma_vel.m diff --git a/anim/rabbit_model/sim_zd.m b/demos/rabbit/anim/rabbit_model/sim_zd.m similarity index 100% rename from anim/rabbit_model/sim_zd.m rename to demos/rabbit/anim/rabbit_model/sim_zd.m diff --git a/anim/rabbit_model/sim_zd_cc.m b/demos/rabbit/anim/rabbit_model/sim_zd_cc.m similarity index 100% rename from anim/rabbit_model/sim_zd_cc.m rename to demos/rabbit/anim/rabbit_model/sim_zd_cc.m diff --git a/anim/rabbit_model/sim_zd_opt.m b/demos/rabbit/anim/rabbit_model/sim_zd_opt.m similarity index 100% rename from anim/rabbit_model/sim_zd_opt.m rename to demos/rabbit/anim/rabbit_model/sim_zd_opt.m diff --git a/anim/rabbit_model/start_of_step_ctrl_sens.m b/demos/rabbit/anim/rabbit_model/start_of_step_ctrl_sens.m similarity index 100% rename from anim/rabbit_model/start_of_step_ctrl_sens.m rename to demos/rabbit/anim/rabbit_model/start_of_step_ctrl_sens.m diff --git a/anim/rabbit_model/step_length.m b/demos/rabbit/anim/rabbit_model/step_length.m similarity index 100% rename from anim/rabbit_model/step_length.m rename to demos/rabbit/anim/rabbit_model/step_length.m diff --git a/anim/rabbit_model/swing_foot_height.m b/demos/rabbit/anim/rabbit_model/swing_foot_height.m similarity index 100% rename from anim/rabbit_model/swing_foot_height.m rename to demos/rabbit/anim/rabbit_model/swing_foot_height.m diff --git a/anim/rabbit_model/swing_foot_velocity.m b/demos/rabbit/anim/rabbit_model/swing_foot_velocity.m similarity index 100% rename from anim/rabbit_model/swing_foot_velocity.m rename to demos/rabbit/anim/rabbit_model/swing_foot_velocity.m diff --git a/anim/rabbit_model/symb_control.m b/demos/rabbit/anim/rabbit_model/symb_control.m similarity index 100% rename from anim/rabbit_model/symb_control.m rename to demos/rabbit/anim/rabbit_model/symb_control.m diff --git a/anim/rabbit_model/symb_model.m b/demos/rabbit/anim/rabbit_model/symb_model.m similarity index 100% rename from anim/rabbit_model/symb_model.m rename to demos/rabbit/anim/rabbit_model/symb_model.m diff --git a/anim/rabbit_model/symb_model_red.m b/demos/rabbit/anim/rabbit_model/symb_model_red.m similarity index 100% rename from anim/rabbit_model/symb_model_red.m rename to demos/rabbit/anim/rabbit_model/symb_model_red.m diff --git a/anim/rabbit_model/theta_obj.m b/demos/rabbit/anim/rabbit_model/theta_obj.m similarity index 100% rename from anim/rabbit_model/theta_obj.m rename to demos/rabbit/anim/rabbit_model/theta_obj.m diff --git a/anim/rabbit_model/tilefigs.m b/demos/rabbit/anim/rabbit_model/tilefigs.m similarity index 100% rename from anim/rabbit_model/tilefigs.m rename to demos/rabbit/anim/rabbit_model/tilefigs.m diff --git a/anim/rabbit_model/total_energy_red.m b/demos/rabbit/anim/rabbit_model/total_energy_red.m similarity index 100% rename from anim/rabbit_model/total_energy_red.m rename to demos/rabbit/anim/rabbit_model/total_energy_red.m diff --git a/anim/rabbit_model/vectfield.m b/demos/rabbit/anim/rabbit_model/vectfield.m similarity index 100% rename from anim/rabbit_model/vectfield.m rename to demos/rabbit/anim/rabbit_model/vectfield.m diff --git a/anim/rabbit_model/zd.m b/demos/rabbit/anim/rabbit_model/zd.m similarity index 100% rename from anim/rabbit_model/zd.m rename to demos/rabbit/anim/rabbit_model/zd.m diff --git a/anim/rabbit_model/zd_cc.m b/demos/rabbit/anim/rabbit_model/zd_cc.m similarity index 100% rename from anim/rabbit_model/zd_cc.m rename to demos/rabbit/anim/rabbit_model/zd_cc.m diff --git a/anim/rabbit_model/zd_ctrl_sens_plots_cc.m b/demos/rabbit/anim/rabbit_model/zd_ctrl_sens_plots_cc.m similarity index 100% rename from anim/rabbit_model/zd_ctrl_sens_plots_cc.m rename to demos/rabbit/anim/rabbit_model/zd_ctrl_sens_plots_cc.m diff --git a/anim/rabbit_model/zd_diff.m b/demos/rabbit/anim/rabbit_model/zd_diff.m similarity index 100% rename from anim/rabbit_model/zd_diff.m rename to demos/rabbit/anim/rabbit_model/zd_diff.m diff --git a/anim/rabbit_model/zd_diff_cc.m b/demos/rabbit/anim/rabbit_model/zd_diff_cc.m similarity index 100% rename from anim/rabbit_model/zd_diff_cc.m rename to demos/rabbit/anim/rabbit_model/zd_diff_cc.m diff --git a/anim/rabbit_model/zd_lift.m b/demos/rabbit/anim/rabbit_model/zd_lift.m similarity index 100% rename from anim/rabbit_model/zd_lift.m rename to demos/rabbit/anim/rabbit_model/zd_lift.m diff --git a/anim/rabbit_model/zd_lift_special.m b/demos/rabbit/anim/rabbit_model/zd_lift_special.m similarity index 100% rename from anim/rabbit_model/zd_lift_special.m rename to demos/rabbit/anim/rabbit_model/zd_lift_special.m diff --git a/anim/rabbit_model/zd_model.mdl b/demos/rabbit/anim/rabbit_model/zd_model.mdl similarity index 100% rename from anim/rabbit_model/zd_model.mdl rename to demos/rabbit/anim/rabbit_model/zd_model.mdl diff --git a/anim/rabbit_model/zd_project.m b/demos/rabbit/anim/rabbit_model/zd_project.m similarity index 100% rename from anim/rabbit_model/zd_project.m rename to demos/rabbit/anim/rabbit_model/zd_project.m diff --git a/anim/rabbit_model/zd_project_cc.m b/demos/rabbit/anim/rabbit_model/zd_project_cc.m similarity index 100% rename from anim/rabbit_model/zd_project_cc.m rename to demos/rabbit/anim/rabbit_model/zd_project_cc.m diff --git a/demos/init_clf_simulation_rabbit.m b/demos/rabbit/init_clf_simulation_rabbit.m similarity index 90% rename from demos/init_clf_simulation_rabbit.m rename to demos/rabbit/init_clf_simulation_rabbit.m index 17e6432..c20e465 100644 --- a/demos/init_clf_simulation_rabbit.m +++ b/demos/rabbit/init_clf_simulation_rabbit.m @@ -46,7 +46,10 @@ dict.clf.rate = 0.7; dict.clf.use_user_defined_rate = false; % set this to true if you don't want to use - dict.epsilon_FL = 0.045; % tune this for controlling convergence rate + % Be careful when tuning this! Low epsilon induces high performance, + % but it can be too aggresive to disregard some physical limitations + % and unknown dynamics + dict.epsilon_FL = 0.07; % Keep it fixed! Fragile! dict.Kp_FL = 1; dict.Kd_FL = 1.8; @@ -55,4 +58,7 @@ % tune A and B to control the weight of each variable dict.weight.input = 1; % A (can be scalar or matrix) dict.weight.slack = 100; % B (should be scalar) + + %% Robot Fall Sensor + dict.fall_threshold = 0.699; end \ No newline at end of file diff --git a/demos/rabbit/plot_rabbit_result.m b/demos/rabbit/plot_rabbit_result.m new file mode 100644 index 0000000..e01eca0 --- /dev/null +++ b/demos/rabbit/plot_rabbit_result.m @@ -0,0 +1,89 @@ +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 + +% Unravel extras data into one array +Vs = []; dVs_error = []; etas_zs = []; +feas = []; slacks = []; mus = []; ys = []; dys = []; Fsts = []; +for i = 1:length(extras) + Vs = [Vs; extras(i).training_datas.Vs(1:end-1)]; + dVs_error = [dVs_error; extras(i).training_datas.dVs_error']; + etas_zs = [etas_zs; extras(i).training_datas.etas_zs(1:end-1, :)]; + dys = [dys; extras(i).dys(1:end-1, :)]; + ys = [ys; extras(i).ys(1:end-1, :)]; + + feas = [feas; extras(i).feas]; + slacks = [slacks; extras(i).slacks]; + mus = [mus; extras(i).mus]; + Fsts = [Fsts; extras(i).Fsts]; +end + +% 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, Fsts(:,1)); grid on; hold on; xlabel('t'); +ylabel('FSt'); plot(ts, Fsts(:,2)); legend(['FSt(1)';'FSt(2)']); + +figure(6); +plot(ts,abs(Fsts(:,1)./Fsts(:,2)), 'color', main_color_blue, 'LineWidth', 1); grid on; hold on; +title('Contact Force Ratio ($|F_{T}$/$F_{N}|$)', 'Interpreter', 'latex'); +xlim([0 ts(end)+1]); +xlabel('Time (sec)'); +ylabel('$|F_{T}$/$F_{N}|$', 'Interpreter', 'latex'); +callPlotSettings(fig_sz, plot_pos); +%print('-f6','plot_F_ratio', '-dpng'); + +figure(7); +plot(ts, us); grid on; hold on; +title('torques'); legend('u1','u2','u3','u4'); + +figure(9); +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'); +end + diff --git a/dynsys_helper/Rabbit_helper/Ce_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/Ce_five_link_walker.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexw64 diff --git a/dynsys_helper/Rabbit_helper/De_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/De_five_link_walker.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexw64 diff --git a/dynsys_helper/Rabbit_helper/Ge_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/Ge_five_link_walker.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexw64 diff --git a/dynsys_helper/Rabbit_helper/J_LeftToe.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/J_LeftToe.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/J_RightToe.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/J_RightToe.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/dJ_RightToe.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/dJ_RightToe.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/p_LeftToe.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/p_LeftToe.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/p_RightToe.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/p_RightToe.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexw64 diff --git a/dynsys_helper/Rabbit_helper/p_Torso.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/p_Torso.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexw64 diff --git a/dynsys_helper/Rabbit_helper/p_q2_left.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/p_q2_left.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexw64 diff --git a/dynsys_helper/Rabbit_helper/p_q2_right.mexw64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexw64 similarity index 100% rename from dynsys_helper/Rabbit_helper/p_q2_right.mexw64 rename to demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexw64 diff --git a/dynsys_helper/Rabbit_helper/trajectory_data.mat b/demos/rabbit/rabbit_helper/Rabbit_helper/trajectory_data.mat similarity index 100% rename from dynsys_helper/Rabbit_helper/trajectory_data.mat rename to demos/rabbit/rabbit_helper/Rabbit_helper/trajectory_data.mat diff --git a/dynsys_helper/readme_rabbit.txt b/demos/rabbit/rabbit_helper/readme_rabbit.txt similarity index 100% rename from dynsys_helper/readme_rabbit.txt rename to demos/rabbit/rabbit_helper/readme_rabbit.txt diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m new file mode 100644 index 0000000..1972434 --- /dev/null +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -0,0 +1,92 @@ +%% RABBIT SIMULATION +% Wonsuhk Jung smw04015@snu.ac.kr +% This is CBF-CLF Helper Version of RABBIT Bipedal Walker. +% You can use a controller designed based on RES-CLF +close all; clear all; clc; + +%% Animation setting +% This is only designed for enabling animation of bipedal walker +global animation_scale + +%% Set Parameters +% Simulation Setting + % sim_t: overall simulation time: do not need it in this mode actually + % dt: time interval to collect data used for plot/animation + % nstep: number of step that RABBIT would take +% params: all the system-related parameters +% verbose: speak about the CBF-CLF(1), silent(0) +% with slack: slack variable in QP(1), no slack variable in QP(0) + +params = init_clf_simulation_rabbit; % Load all the parameters +dt = 0.025; % simulation interval +sim_t = 100; % simulation time +nstep = 10; % number of steps +with_slack = true; % use slack variable in QP (change this into false in order not to use it) +verbose = false; % show the QP-solving process + +%% Initialize state +% Init state. +q0_ini = params.legacy.q0_ini; +dq0_ini = params.legacy.dq0_ini; +x0 = [q0_ini;dq0_ini]; +% Relabeling matrix (reset map) +R = [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]; + +%% Prepare System (Uncertainty control) +control_sys = RabbitBuiltIn(params); +plant_sys = RabbitBuiltIn(params); + +% Reflect model uncertainty here +plant_sys.params.scale = 1.5; +%plant_sys.params.torso_add = 10; + +controller = @control_sys.ctrlClfQpFL; +event_options = odeset('Events', @plant_sys.rabbit_event); + +%% Prepare record paper +xs = []; +ts = []; +us = []; +extras = []; + +% Initialize the state and time +x = x0; +t0 = 0; +%% Main Simulation +for k = 1:nstep + step_summary = strcat("[Step]", num2str(k)); disp(step_summary); + + [xs_new, us_new, ts_new, extras_new] = rollout_controller_eval_clf_FL( ... + x0, t0, plant_sys, control_sys, controller, dt, ..., + 'event_options', event_options, 'sim_t', sim_t, 'with_slack', with_slack, ..., + 'verbose', verbose); + + % Reset Map when event is detected + n = plant_sys.params.xdim/2; + q = R * xs_new(end, 1:n)'; + dq = R*plant_sys.dq_pos_gen_v2([xs_new(end, 1:n) xs_new(end, n+1:2*n)]); + + % Renew the new state + x0 = [q; dq]; + t0 = ts_new(end); + + ts = [ts; ts_new(1:end-1)]; + xs = [xs; xs_new(1:end-1, :)]; + us = [us; us_new]; + extras = [extras; extras_new]; +end +%% Plot the result +% TODO: tune the plot setting into new version +plot_rabbit_result(xs, ts, us, extras); + +%% Five Link Animation +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/run_clf_simulation_rabbit.m b/demos/run_clf_simulation_rabbit.m deleted file mode 100644 index ed92f98..0000000 --- a/demos/run_clf_simulation_rabbit.m +++ /dev/null @@ -1,151 +0,0 @@ -%% RABBIT SIMULATION -% This is CBF-CLF Helper Version of RABBIT Bipedal Walking. -% This version now supports -close all; clear all; clc; -%% Animation setting -global animation_scale % animation scale declared global - -%% Set Parameters -% Simulation Setting - % sim_t: overall simulation time: do not need it in this mode actually - % dt: time interval to collect data used for plot/animation - % nstep: number of step that RABBIT would take -% params: all the system-related parameters -% verbose: speak about the CBF-CLF(1), silent(0) -% with slack: slack variable in QP(1), no slack variable in QP(0) - -params = init_clf_simulation_rabbit; % Load all the parameters -dt = 0.01; % simulation interval -sim_t = 100; % simulation time -nstep = 10; % number of steps -with_slack = true; % use slack variable in QP (change this into false in order not to use it) -verbose = false; % show the QP-solving process - -%% Initialize state -% Init state. -q0_ini = params.legacy.q0_ini; -dq0_ini = params.legacy.dq0_ini; -x0 = [q0_ini;dq0_ini]; -% Relabeling matrix (reset map) -R = [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]; - -%% Prepare System -control_sys = RabbitBuiltIn(params); -plant_sys = RabbitBuiltIn(params); -plant_sys.params.scale = 3; % Reflect model uncertainty here -%plant_sys.params.torso_add = 10; -controller = @control_sys.ctrlClfQpFL; -event_options = odeset('Events', @plant_sys.rabbit_event); -%odeFun = @plant_sys.dynamics; -%odeSolver = @ode45; - -%% Prepare record paper -total_k = ceil(sim_t / dt); -x = x0; -t0 = 0; -% initialize traces. -% xs = zeros(total_k, dynsys.xdim); -xs = []; -ts = []; -us = zeros(total_k-1, control_sys.udim); -hs = zeros(total_k-1, 1); -Vs = zeros(total_k-1, 1); -Fsts = zeros(total_k-1, 2); - -xs(1, :) = x0'; -ts(1) = t0; - -%% Main Simulation -for k = 1:nstep - step_summary = strcat("[Step]", num2str(k)); - disp(step_summary); - - [xs_new, us_new, ts_new, Vs_new, ~, ys_new] = rollout_feedback_linearization( ... - x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, [], verbose, event_options); - - % Reset Map when event is detected - n = params.xdim/2; - q = R * xs_new(end, 1:n)'; - dq = R*plant_sys.dq_pos_gen_v2([xs_new(end, 1:n) xs_new(end, n+1:2*n)]); - - % Renew the new state - x0 = [q; dq]; - t0 = ts_new(end); - - ts = [ts; ts_new(2:end)]; - xs = [xs; xs_new(2:end, :)]; -end -%% New Figures -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(2:end), plant_sys.y_out_history(:,1)); grid on; -xlabel('Time(s)'); ylabel('q_1'); -title('Stance Leg Hip Angle'); - -figure(2); -plot(ts(2:end),plant_sys.y_out_history(:,2)); grid on; -xlabel('Time(s)'); ylabel('q_3'); -title('Stance Leg Knee Angle'); - -figure(3); -plot(ts(2:end),plant_sys.y_out_history(:,3)); grid on; -xlabel('Time(s)'); ylabel('q_2'); -title('Swing Leg Hip Angle'); - -figure(4); -plot(ts(2:end),plant_sys.y_out_history(:,4)); grid on; -xlabel('Time(s)'); ylabel('q_4'); -title('Swing Leg Knee Angle'); - -figure(5); plot(ts(2:end), plant_sys.F_st_history(:,1)); grid on; hold on; xlabel('t'); -ylabel('FSt'); plot(ts(2:end), plant_sys.F_st_history(:,2)); legend(['FSt(1)';'FSt(2)']); - -figure(6); -plot(ts(2:end),abs(plant_sys.F_st_history(:,1)./plant_sys.F_st_history(:,2)), 'color', main_color_blue, 'LineWidth', 1); grid on; hold on; -title('Contact Force Ratio ($|F_{T}$/$F_{N}|$)', 'Interpreter', 'latex'); -xlim([0 ts(end)+1]); -xlabel('Time (sec)'); -ylabel('$|F_{T}$/$F_{N}|$', 'Interpreter', 'latex'); -callPlotSettings(fig_sz, plot_pos); -%print('-f6','plot_F_ratio', '-dpng'); - -figure(7); -plot(ts(2:end),plant_sys.u_motor_history); grid on; hold on; -title('torques'); legend('u1','u2','u3','u4'); - -figure(9); -subplot1(2,1,'Gap', subplot_gap); -subplot1(1); hold on; -title('Euclidian Norm of the Tracking Error'); -plot(ts(2:end),sqrt(plant_sys.y_out_history(:,1).^2+plant_sys.y_out_history(:,2).^2+... - plant_sys.y_out_history(:,3).^2+plant_sys.y_out_history(:,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(2:end),sqrt(plant_sys.dy_out_history(:,1).^2+plant_sys.dy_out_history(:,2).^2+... - plant_sys.dy_out_history(:,3).^2+plant_sys.dy_out_history(:,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'); -%% Five Link Animation -animation_scale = plant_sys.params.scale; -animation_dt = 0.05; -x_quan_vec = coordinateTransformation(xs); -anim_flat_ground(ts, x_quan_vec, animation_dt) diff --git a/demos/two_link_simulate.m b/demos/two_link_simulate.m deleted file mode 100644 index de2147b..0000000 --- a/demos/two_link_simulate.m +++ /dev/null @@ -1,155 +0,0 @@ -function [xRec, fRec, uRec, tRec, xRec_tilde] = two_link_simulate(x0, beta, Nsteps, th1d, sim_option, sim_event_option) -%TWO_LINK_SIMULATE simulates the two link robot for Nsteps number of -%walking steps and with I/O parameters beta and initial condition x0. -% Ayush Agrawal, Jason Choi -% ayush.agrawal@berkeley.edu -% jason.choi@berkeley.edu -% Inputs: x0: initial state of the step in full representation in the original coordinate (8 x 1 vector) -% beta: bezier coefficients that define the reference gait -% Nsteps: number of walking steps to simulate -% th1d: desired value of q1 (phase var) at the end of a step -% sim_option: simulation option. -% 'vanilla': use two_link_dynamics.m to run simulation. -% 'tilde': use two_link_dynamics_tilde.m to run simulation. -% 'vanilla_class': use CompassWalker.dynamics to run simulation. -% 'tilde_class': use CompassWalkerTilde.dynamics to run simulation. -% sim_event_option: event used for reset map in simulation. -% 'angle': Reset is defined as q1 reaching q1_desired. -% 'foot': Reset is defined as foot position reaching the -% ground (y=0). - -if nargin<1 || isempty(x0) - x0 = getInitialCondition(); -end -if nargin<3 - Nsteps = 1; -end -if nargin<4 - th1d = -x0(3); -end -if nargin<5 - sim_option = 'vanilla'; -end -if nargin<6 - sim_event_option = 'foot'; -end - -% variables to record data -tRec = 0; -xRec = []; -xRec_tilde = []; -% parameters -params.ctrl_type = 'io'; -params.beta = beta; -params.eps = 0.1; % Epsilon used in feedback linearization. -params.th1d = th1d; -params.reset_q1_threshold = -0.03; - -% Reflection matrix (swap leg1 and 2) -R = two_link_reflect; - -for i = 1:Nsteps - step_summary.x_init = x0; - [xs, ts, xs_tilde] = step_simulate_helper(x0, tRec(end), params, sim_option, sim_event_option); - - % Record data - tRec = [tRec;ts]; - xRec = [xRec;xs]; - xRec_tilde = [xRec_tilde;xs_tilde]; - step_summary.x_terminal = xs(end, :)'; - % Check swing foot position - pSw_at_event = pSw_gen(xs(end, :)'); - step_summary.pSw_terminal = pSw_at_event; - - print_step_summary(i, step_summary); - - %% Apply reset map and reinitialize initial state for next step. - q_minus = xs(end, 1:4)'; - dq_minus = xs(end, 5:end)'; - q_plus = R*q_minus; - dq_plus = R*dqPlus_gen([q_minus;dq_minus]); - x0 = [q_plus;dq_plus]; -end - -tRec = tRec(2:end); -uRec = []; -fRec = []; -%% Compute control inputs and Contact forces -for i = 1:length(tRec) - u = two_link_io_control(xRec(i, :)', params); - uRec = [uRec; u]; - fRec = [fRec; Fst_gen(xRec(i, :)', u)']; -end -end - -function [xs, ts, xs_tilde] = step_simulate_helper(x0, t0, params, sim_option, event_option) -%% function that runs walk step simluation -% Inputs: x0: initial state of the step in full representation in the original coordinate (8 x 1 vector) -% t0: initial time of the step -% params: control params -% .ctrl_type -% .beta -% .eps -% .th1d -% sim_option: simulation option -% 'vanilla': use two_link_dynamics.m to run simulation. -% 'tilde': use two_link_dynamics_tilde.m to run simulation. -% 'vanilla_class': use CompassWalker.dynamics to run simulation. -% 'tilde_class': use CompassWalkerTilde.dynamics to run simulation. - tspan = [t0, t0+3]; - gait_params = [params.beta;params.th1d]; - if strcmp(sim_option, 'vanilla') - dynamics_function = @(t,x)two_link_dynamics(t, x, params); - event_function = @(t, x)two_link_event(t, x, params, event_option); - odeOpts = odeset('Events', event_function); - [ts, xs] = ode45(dynamics_function, tspan, x0, odeOpts); - xs_tilde = xs_tilde_from_original(xs, gait_params); - elseif strcmp(sim_option, 'vanilla_class') - dynsys = CompassWalker(1:4, gait_params, [], event_option); - dynamics_function = @(t,x) dynsys.dynamics(t, x, [], [], true, params); - event_function = @dynsys.reset_event; - odeOpts = odeset('Events', event_function); - x0_condensed = x0([3, 4, 7, 8]); - init_stance_position = pSt_gen(x0); - [ts, xs_condensed] = ode45(dynamics_function, tspan, x0_condensed, odeOpts); - xs = convert_condensed_states_to_full_states( … - xs_condensed, init_stance_position); - xs_tilde = xs_tilde_from_original(xs, gait_params); - elseif strcmp(sim_option, 'tilde') - x0_tilde = [q_tilde_from_q_gen(x0(1:4), gait_params); - dq_tilde_from_qdq_gen(x0(1:4), x0(5:8), gait_params)]; - dynamics_function = @(t,x)two_link_dynamics_tilde(t, x, params); - event_function = @(t, x)two_link_event_tilde(t, x, params, event_option); - odeOpts = odeset('Events', event_function); - [ts, xs_tilde] = ode45(dynamics_function, tspan, x0_tilde, odeOpts); - xs = xs_original_from_tilde(xs_tilde, gait_params); - elseif strcmp(sim_option, 'tilde_class') - dynsys = CompassWalkerTilde(1:4, gait_params, [], event_option); - dynamics_function = @(t,x) dynsys.dynamics(t, x, [], [], true, params); - event_function = @dynsys.reset_event; - odeOpts = odeset('Events', event_function); - init_stance_position = pSt_gen(x0); - x0_tilde = [q_tilde_from_q_gen(x0(1:4), gait_params); - dq_tilde_from_qdq_gen(x0(1:4), x0(5:8), gait_params)]; - x0_condensed_tilde = x0_tilde([3, 4, 7, 8]); - [ts, xs_condensed_tilde] = ode45( … - dynamics_function, tspan, x0_condensed_tilde, odeOpts); - xs_tilde_temp = zeros(length(ts), 8); - xs_tilde_temp(:, [3,4,7,8]) = xs_condensed_tilde; - xs_original_temp = xs_original_from_tilde(xs_tilde_temp, gait_params); - xs_condensed = xs_original_temp(:, [3, 4, 7, 8]); - xs = convert_condensed_states_to_full_states( … - xs_condensed, init_stance_position); - xs_tilde = xs_tilde_from_original(xs, gait_params); - 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'); - disp("Swing foot position at terminal state (x, y)"); - disp(log.pSw_terminal'); -end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index 16f98d8..468a79b 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -25,6 +25,43 @@ function clear_properties(obj) obj.dy_out_history = []; obj.u_motor_history = []; 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.gen_DCGB_rel(q); + %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 + 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]; + 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 @@ -32,11 +69,26 @@ function clear_properties(obj) % inevitable or else, we can have another prior function but % still we need to waste memory space! % memory <-> time complexity issue - [f_,~] = obj.defineSystem(x); + [f_,~] = obj.defineDynamics(x); end function g_ = g(obj, x) % RABBIT g(x) - [~, g_] = obj.defineSystem(x); + [~, g_] = obj.defineDynamics(x); + end + function z_ = z(obj, x) + % zero dynamics + n = obj.xdim/2; + q = x(1:n); + dq = x(n+1:2*n); + + theta = q(3) + q(4) + q(5)/2; +% theta_init = obj.params.legacy.theta_init; +% theta_end = obj.params.legacy.theta_end; +% phase = min(max((theta - theta_init)/(theta_end - theta_init))); + + dtheta = dq(3) + dq(4) + dq(5)/2; + + z_ = [theta dtheta]; end function B = cbf(obj, x) % TODO: CBF(x) diff --git a/dynsys/@RabbitBuiltIn/defineSystem.m b/dynsys/@RabbitBuiltIn/defineSystem.m deleted file mode 100644 index 2926de8..0000000 --- a/dynsys/@RabbitBuiltIn/defineSystem.m +++ /dev/null @@ -1,44 +0,0 @@ -function [f, g] = defineSystem(obj, x) -% define f and g of the dynamical system -% Forked from RABBIT_IO_RL -% f and g would be calculated - duplicated -% => TODO: should be incorporated - n = obj.xdim / 2; - if obj.params.mode.motor_dynamics - u_s = x(end-3 : end); - end - - q = x(1:n); - dq = x(n+1:2*n); - - %[D, C, G, B] = obj.gen_DCGB_rel(q); - %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 - 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]; - 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 - diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 1da5f1a..f31e6d2 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -17,6 +17,7 @@ % State dimension xdim + sdim % Control input dimension udim % Number of clf in use @@ -172,10 +173,12 @@ 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); + %cbf_ = zeros(obj.n_cbf, 1); + cbf_ = []; for i = 1:obj.n_cbf cbf_i = obj.cbf_sym{i}(x); - cbf_(i) = cbf_i; + %cbf_(i) = cbf_i; + cbf_ = [cbf_; cbf_i]; end end diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 333621d..16344f1 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -30,6 +30,7 @@ function init_sys(obj, params) 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); obj.f_sym = matlabFunction(f_, 'vars', {x}); obj.g_sym = matlabFunction(g_, 'vars', {x}); @@ -102,6 +103,7 @@ function init_sys(obj, params) 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 diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index f6f0764..47dee28 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -40,6 +40,7 @@ if nargin < 1 error("Warning: params argument is missing.") end + disp(setup_option) if nargin < 2 setup_option = 'symbolic'; end diff --git a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m index ba29b94..e344289 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, slack, V, feas] = ctrlClfQpFL(obj, s, mu_ref, with_slack, verbose) + %% 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) @@ -26,7 +26,7 @@ error("Wrong size of mu_ref, it should be (udim, 1) array."); end - [y, dy, L2fy, LgLfy, phase] = obj.eval_y(s); + [y, dy, L2fy, LgLfy, ~] = obj.eval_y(s); V = obj.clf_FL(y, dy); LfV = obj.lF_clf_FL(y, dy); diff --git a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m index c992711..b2aa912 100644 --- a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m +++ b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m @@ -27,17 +27,18 @@ u = u_raw; %% Clip input into the physical constraint +eps = 1e-4; if isfield(obj.params, 'u_max') if size(obj.params.u_max, 1) == 1 % row vector for i = 1:obj.udim - if u(i) > obj.params.u_max + if u(i) > obj.params.u_max + eps u(i) = obj.params.u_max; disp("Warning: virtual control input does not satisfy input constraint.") end end elseif size(obj.params.u_max, 1) == obj.udim % column vector for i = 1:obj.udim - if u(i) > obj.params.u_max(i) + if u(i) > obj.params.u_max(i) + eps u(i) = obj.params.u_max(i); disp("Warning: virtual control input does not satisfy input constraint.") end @@ -50,14 +51,14 @@ 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 + if u(i) < obj.params.u_min - eps u(i) = obj.params.u_min; 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) + if u(i) < obj.params.u_min(i) - eps u(i) = obj.params.u_min(i); disp("Warning: virtual control input does not satisfy input constraint.") end diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index 41f03ce..af8bd7a 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -32,6 +32,9 @@ function init_sys_FL(obj, params) 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; if ~isfield(params, 'Kp_FL') error("Define variable in the params called 'Kp_FL', p gain for RES-CLF"); end diff --git a/lib/@CtrlAffineSysFL/rollout_controller_eval_clf_FL.m b/lib/@CtrlAffineSysFL/rollout_controller_eval_clf_FL.m new file mode 100644 index 0000000..b69a434 --- /dev/null +++ b/lib/@CtrlAffineSysFL/rollout_controller_eval_clf_FL.m @@ -0,0 +1,168 @@ +% function [xs, us, mus, ts, Vs, dVs_error, etas_zs, feas, slacks, extras] = rollout_controller_eval_clf_FL( ... +% x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_options) +function [xs, us, ts, extras] = rollout_controller_eval_clf_FL( ... + x0, t0, plant_sys, control_sys, controller, dt, varargin) +%% Parse varargin into settings +% Support following symbols for varargin + % with_slack + % mu0 = reference for mu input + % verbose = 0(run quitely), 1(show QP info) + % event_options = run simulation until event happens + % sim_t = simulation time + +settings = parse_function_args(varargin{:}); +if ~isfield(settings, 'with_slack') + with_slack = 0; +else + with_slack = settings.with_slack; +end + +if ~isfield(settings, 'mu0') + mu0 = []; +else + mu0 = settings.mu0; +end + +if ~isfield(settings, 'verbose') + verbose = 0; +else + verbose = settings.verbose; +end + +if ~isfield(settings, 'event_options') + event_options = 0; +else + event_options = settings.event_options; +end + +if ~isfield(settings, 'sim_t') + % the dynamics that engage reset map should not be judged by sim_t + % set sim_t to enough time + sim_t = 100; +else + sim_t = settings.sim_t; +end + +%% Prepare record paper +% Total time step +total_k = ceil(sim_t / dt); + +% Initialize traces. +xs = zeros(1, plant_sys.xdim); % trace of the simulation. +ys = zeros(1, control_sys.ydim); % Wonsuhk Revised +ts = zeros(2, 1); +Vs = zeros(2, 1); +dys = zeros(1, control_sys.ydim); % Wonsuhk Revised +us = zeros(1, control_sys.udim); +mus = zeros(1, control_sys.udim); +dVs_error = zeros(1, 1); +feas = false(1, 1); % added +slacks = zeros(1, 1); % added +F_sts = zeros(1, 2); % added + +x = x0; xs(1, :) = x0; +t = t0; ts(1) = t; + +[y, dy, ~, ~, ~] = control_sys.eval_y(x); + +z = control_sys.z(x); +ys(1, :) = y; +dys(1, :) = dy; +Vs(1) = control_sys.clf_FL(y, dy); +zs(1, :) = z; +%% Run Simulation + +for k = 1:total_k-1 + % Evaluate CLf related values + z = control_sys.z(x); + V = control_sys.clf_FL(y, dy); + LfV = control_sys.lF_clf_FL(y, dy); + LgV = control_sys.lG_clf_FL(y, dy); + + %% Determine control input + if k ==1 && ~isempty(mu0) + % Apply predetermined control input for the initial step. + mu = mu0; %mu0 + u = control_sys.ctrlFeedbackLinearize(x, mu); + dV_hat = LfV + LgV * mu; % model based estimate of dV + feas_k = true; + slack = 0; + else + % Determin control input from the controller. + [mu, slack, V, feas_k] = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); % mu + u = control_sys.ctrlFeedbackLinearize(x, mu); % u=u_star + LgLf\mu + dV_hat = LfV + LgV * mu; % model based estimate of dV + end + feas(k, :) = feas_k; + us(k, :) = u; + mus(k, :) = mu; + slacks(k, :) = slack; + + % Run simulation for one time step with determined control input. + if isempty(event_options) + [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! + else + [ts_k, xs_k, te, xe, ~] = ... + ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! + end + + t = ts_k(end); + ts(k+1) = t; + x_next = xs_k(end, :)'; + % x_next = clip_theta(xs_k(end, :))'; + xs(k+1, :) = x_next; + + %% Recording Section: Evaluate CLF related values after one time step. + % Record dVs_error(k) = dV - dV_hat + % Vs = V_next + [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); + z_next = control_sys.z(x_next); + ys(k+1, :) = y_next; % Wonsuhk_revised + dys(k+1, :) = dy_next; + V_next = control_sys.clf_FL(y_next, dy_next); + LfV_next = control_sys.lF_clf_FL(y_next, dy_next); + LgV_next = control_sys.lG_clf_FL(y_next, dy_next); + % Estimating Vdot based on second order numerical differentiation. + dV_hat_next = LfV_next + LgV_next * mu; + dV_hat = (dV_hat + dV_hat_next)/2; + % Estimation of actual Vdot based on numerical differentiation + dV = (V_next - V) / dt; + dVs_error(k) = dV - dV_hat; + Vs(k+1) = V_next; + zs(k+1, :) = z_next; + + [F_st_u_next, F_st_nu_next] = plant_sys.get_force(x_next); + F_st_next = F_st_u_next*u + F_st_nu_next; + F_sts(k, :) = F_st_next'; + + if verbose + fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... + [dV, dV_hat, dV-dV_hat, LfV, x(3), x(4)]); + end + + if ~isempty(te), break; end + + x = x_next; + y = y_next; + dy = dy_next; + % phase = phase_next; + +end % end of main for loop + +% Cache the training data in aggregated form +etas = [ys,dys]; +etas_zs = [etas zs]; + +%% Archive extra datas +% training data required to train the model (refer to GP-CLF-SOCP ver) +extras.training_datas.Vs = Vs; +extras.training_datas.dVs_error = dVs_error; +extras.training_datas.etas_zs = etas_zs; + +% Tune this in order to retrieve the specific params you want to save +extras.feas = feas; +extras.slacks = slacks; +extras.mus = mus; +extras.ys = ys; +extras.dys = dys; +extras.Fsts = F_sts; \ No newline at end of file diff --git a/utils/rollout_feedback_linearization.m b/utils/rollout_feedback_linearization.m deleted file mode 100644 index cf427b7..0000000 --- a/utils/rollout_feedback_linearization.m +++ /dev/null @@ -1,121 +0,0 @@ -function [xs, us, ts, Vs, dVs_error, ys] = rollout_feedback_linearization( ... - x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_options) -% controller = CBF-CLF-QP -if nargin < 7 - with_slack = 0; - -elseif isempty(with_slack) - with_slack = 0; -end - -if nargin < 8 - mu0 = []; -end - -if nargin < 9 - verbose = 0; % run queitly -end - -if nargin < 10 - event_options = []; % 0: no event function -end - -% Total time step -total_k = ceil(sim_t / dt); - -% Initialize traces. -xs = zeros(2, plant_sys.xdim); % trace of the simulation. -ys = zeros(2, control_sys.ydim); % Wonsuhk Revised -us = zeros(1, control_sys.udim); -ts = zeros(2, 1); -Vs = zeros(2, 1); -dVs_error = zeros(1, 1); - -x = x0; xs(1, :) = x0; -t = t0; ts(1) = t; % wonsuhk revised - -[y, dy, ~, ~, phase] = control_sys.eval_y(x); -ys(1, :) = y; -Vs(1) = control_sys.clf_FL(y, dy); -%% Run Simulation -for k = 1:total_k-1 - % Evaluate CLf related values - V = control_sys.clf_FL(y, dy); - LfV = control_sys.lF_clf_FL(y, dy); - LgV = control_sys.lG_clf_FL(y, dy); - - %% Determine control input - if k ==1 && ~isempty(mu0) - % Apply predetermined control input for the initial step. - mu = mu0; %mu0 - u = control_sys.ctrlFeedbackLinearize(x, mu); - dV_hat = LfV + LgV * mu; % model based estimate of dV - else - % Determin control input from the controller. - mu = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); - u = control_sys.ctrlFeedbackLinearize(x, mu); % u=u_star + LgLf\mu - %% for debug; -% u = 0; -% feedforward = -control_sys.lglf_y(x)\control_sys.l2f_y(x); -% mu = control_sys.lglf_y(x) * (u - feedforward); - dV_hat = LfV + LgV * mu; % model based estimate of dV - end - us(k, :) = u; - - % Run simulation for one time step with determined control input. - if isempty(event_options) - [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! - else - [ts_k, xs_k, te, ~, ~] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! - end - - t = ts_k(end); - ts(k+1) = t; - x_next = xs_k(end, :)'; - % x_next = clip_theta(xs_k(end, :))'; - xs(k+1, :) = x_next; - - %% Recording Section: Evaluate CLF related values after one time step. - % Record dVs_error(k) = dV - dV_hat - % Vs = V_next - [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); - ys(k+1, :) = y_next; % Wonsuhk_revised - V_next = control_sys.clf_FL(y_next, dy_next); - LfV_next = control_sys.lF_clf_FL(y_next, dy_next); - LgV_next = control_sys.lG_clf_FL(y_next, dy_next); - % Estimating Vdot based on second order numerical differentiation. - dV_hat_next = LfV_next + LgV_next * mu; - dV_hat = (dV_hat + dV_hat_next)/2; - % Estimation of actual Vdot based on numerical differentiation - dV = (V_next - V) / dt; - dVs_error(k) = dV - dV_hat; - Vs(k+1) = V_next; -% if abs(dV-dV_hat) > 50 -% disp("debug"); -% end -%% Under examination - [F_st_u_next, F_st_nu_next] = plant_sys.get_force(x_next); - F_st_next = F_st_u_next*u + F_st_nu_next; - - plant_sys.y_out_history = [plant_sys.y_out_history; y_next']; - plant_sys.dy_out_history = [plant_sys.dy_out_history; dy_next']; - plant_sys.F_st_history = [plant_sys.F_st_history; F_st_next']; - plant_sys.u_motor_history = [plant_sys.u_motor_history; u']; - - if verbose - mu' - fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... - [dV, dV_hat, dV-dV_hat, LfV, x(3), x(4)]); - LgV - end - - if ~isempty(te) - disp("Touched Ground"); - break; - end - - x = x_next; - y = y_next; - dy = dy_next; - phase = phase_next; -end % end of main for loop \ No newline at end of file From d5329485c9e08816446e00c61b338f553b00726a Mon Sep 17 00:00:00 2001 From: jason Date: Sun, 3 Oct 2021 18:17:42 -0700 Subject: [PATCH 13/49] refactor controller function io and add general simulation interface. --- demos/run_cbf_clf_simulation_2DDI.m | 59 +---- demos/run_cbf_clf_simulation_acc.m | 68 ++---- demos/run_cbf_clf_simulation_dubins_car.m | 71 ++---- demos/run_clf_simulation_inverted_pendulum.m | 42 +--- lib/@CtrlAffineSys/CtrlAffineSys.m | 74 +++--- lib/@CtrlAffineSys/ctrlCbfClfQp.m | 17 +- lib/@CtrlAffineSys/ctrlCbfQp.m | 11 +- lib/@CtrlAffineSys/ctrlClfQp.m | 14 +- lib/@CtrlAffineSys/init_sys.m | 8 +- lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 3 + lib/@CtrlAffineSysFL/init_sys_FL.m | 8 +- lib/sim/rollout_controller.m | 225 ++++++++++++++++++ .../rollout_controller_eval_clf_FL.m | 0 utils/io/parse_function_args.m | 7 + utils/io/parse_obj_args.m | 6 + 15 files changed, 381 insertions(+), 232 deletions(-) create mode 100644 lib/sim/rollout_controller.m rename lib/{@CtrlAffineSysFL => sim}/rollout_controller_eval_clf_FL.m (100%) create mode 100644 utils/io/parse_function_args.m create mode 100644 utils/io/parse_obj_args.m diff --git a/demos/run_cbf_clf_simulation_2DDI.m b/demos/run_cbf_clf_simulation_2DDI.m index 13447ba..6e484b5 100644 --- a/demos/run_cbf_clf_simulation_2DDI.m +++ b/demos/run_cbf_clf_simulation_2DDI.m @@ -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 250202f..363706a 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); +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..c5a90cc 100644 --- a/demos/run_cbf_clf_simulation_dubins_car.m +++ b/demos/run_cbf_clf_simulation_dubins_car.m @@ -15,92 +15,57 @@ % 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, 100, 100]; 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 +controller = @dubins.ctrlCbfClfQp; -plot_results(ts, xs, us, hs, [params.xo;params.yo], params.d) +[xs, us, ts, extraout] = rollout_controller( ... + x0, dubins, dubins, controller, sim_t); +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 +76,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_clf_simulation_inverted_pendulum.m b/demos/run_clf_simulation_inverted_pendulum.m index 0af360a..dd870dd 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); 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/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index f31e6d2..178c45b 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -33,7 +33,6 @@ % (Always preferred to refer directly to the class properties.) params config - use_phase %% Functions generated from symbolic expressions. % (Used when setup_option is 'symbolic'.) @@ -54,6 +53,19 @@ %% dynsys = CtrlAffineSys(params, 'symbolic') %% if user-defined dynamics, cbf, clf are used: %% dynsys = CtrlAffineSys(params, 'built-in') + % 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 + % Optional fields: + % u_min: control min bound (scalar, or the vector of size u_min) + % u_max: control max bound if nargin < 1 error("Warning: params argument is missing.") end @@ -103,7 +115,7 @@ 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). @@ -114,7 +126,7 @@ 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). @@ -124,89 +136,93 @@ 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). + % Vs: (size: (obj.n_clf, 1)) 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); + Vs = zeros(obj.n_clf, 1); 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)}. + % LfVs: (size: (obj.n_clf, 1)) 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); + LfVs = zeros(obj.n_clf, 1); 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)}. + % LgVs: (size: (obj.n_clf, obj.udim)) 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); + LgVs = zeros(obj.n_clf, obj.udim); for i = 1:obj.n_clf lg_clf_i = obj.lg_clf_sym{i}(x); - lg_clf_(i, :) = lg_clf_i; + LgVs(i, :) = lg_clf_i; end end - function cbf_ = cbf(obj, x) - % cbf_ = cbf(obj, x) + 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). + % Bs: (size: (obj.n_cbf, 1)) 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); - cbf_ = []; + Bs = zeros(obj.n_cbf, 1); for i = 1:obj.n_cbf cbf_i = obj.cbf_sym{i}(x); - %cbf_(i) = cbf_i; - cbf_ = [cbf_; 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)}. + % LfBs: (size: (obj.n_cbf, 1)) 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); + LfBs = zeros(obj.n_cbf, 1); 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)}. + % LgBs: (size: (obj.n_cbf, obj.udim)) 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); + LgBs = zeros(obj.n_cbf, obj.udim); for i = 1:obj.n_cbf lg_cbf_i = obj.lg_cbf_sym{i}(x); - lg_cbf_(i, :) = lg_cbf_i; + LgBs(i, :) = lg_cbf_i; end end end diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index b2aee29..ba4e7d9 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -1,14 +1,15 @@ %% 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 +function [u, extraout] = ctrlCbfClfQp(obj, x, u_ref, with_slack, verbose) + %% Implementation of the 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. + % 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. @@ -127,4 +128,10 @@ 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..6984167 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -1,11 +1,13 @@ %% Author: Jason Choi (jason.choi@berkeley.edu) -function [u, slack, Bs, feas, comp_time] = ctrlCbfQp(obj, x, u_ref, verbose, with_slack) +function [u, extraout] = 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. + % 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. @@ -105,4 +107,9 @@ 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..f8c068c 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -1,13 +1,14 @@ %% 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 +function [u, extraout] = ctrlClfQp(obj, x, u_ref, with_slack, verbose) + %% Implementation of the 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. + % 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. @@ -119,4 +120,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 16344f1..56d7923 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -4,13 +4,7 @@ function init_sys(obj, params) % 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 ~isfield(params, 'use_phase') - obj.use_phase = 1; - else - obj.use_phase = params.use_phase; - end - + if strcmp(obj.setup_option, 'symbolic') disp(['Setting up the dynamics, CLFs, CBFs from defined symbolic expressions.', ... '(This might take time.)']); diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index 47dee28..d5e783c 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -31,6 +31,9 @@ lglf_y_min_exceed_sym l2f_y_min_exceed_sym + % binary indicator on whether it uses phase-based output or not. + use_phase + % CLF under feedback linearization and its derivatives as function handles. Gram_clf_FL % Gram matrix of the CLF for feedback linearization. end diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index af8bd7a..99778db 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -1,5 +1,11 @@ function init_sys_FL(obj, params) - %% Functions that initialize dynamic system +%% Functions that initialize dynamic system + if ~isfield(params, 'use_phase') + obj.use_phase = 1; + else + obj.use_phase = params.use_phase; + end + %% Symbolic Function if strcmp(obj.setup_option, 'symbolic') disp(['Setting up feedback linearization dynamics, CLFs, CBFs from defined symbolic expressions.', ... diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m new file mode 100644 index 0000000..0805499 --- /dev/null +++ b/lib/sim/rollout_controller.m @@ -0,0 +1,225 @@ +function [xs, us, ts, extraout] = rollout_controller( ... + x0, plant_sys, control_sys, controller, T, varargin) +%% [xs, us, ts, extras] = rollout_controller( ... +%% x0, t0, plant_sys, control_sys, controller, T, varargin) +%% [xs, us, ts, extras] = rollout_controller( ... +%% x0, t0, 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; +%% 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 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) +% ratio_u_diff: ratio(0.0~1.0) in the min-norm objective for minimizing the +% difference between u and u_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}) +% u_ref: reference signal of u. It can be a constant vector or a function +% handle that takes x as input. +% DEBUG: +% verbose: print log if 1 +%% 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) + +settings = parse_function_args(varargin{:}); +if ~isfield(settings, 't0') + t0 = 0; +else + t0 = settings.t0; +end + +if ~isfield(settings, 'u0') + u0 = []; +else + if length(u0) ~= control_sys.udim + error("u0 dimension does not match with control_sys.udim."); + end + u0 = settings.u0; +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, 'ratio_u_diff') + ratio_u_diff = 0; +else + ratio_u_diff = settings.ratio_u_diff; +end + +if ~isfield(settings, 'u_ref') + u_ref = []; +else + if isa(settings.u_ref, 'function_handle') + error("Not supported in the current version."); + elseif 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, 'verbose') + verbose = 0; +else + verbose = settings.verbose; +end + +if ~isfield(settings, 'end_event') + end_event = []; +else + end_event = settings.end_event; +end + +if ~isfield(settings, 'ode_func') + ode_func = @ode45; +else + ode_func = settings.ode_func; +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 = []; +Vs = []; +Bs = []; +feas = []; +slacks = []; +comp_times = []; + +% Initialize state & time. +x = x0; +t = t0; +u_prev = zeros(control_sys.udim, 1); +end_simulation = false; +%% Run simulation. +% _t indicates variables for the current loop. +while ~end_simulation + %% Determin control input. + if t == t0 && ~isempty(u0) + u = u0; + else + if isempty(u_ref) + u_ref_t = ratio_u_diff * u_prev; + else + u_ref_t = u_ref; + end + [u, extra_t] = controller(x, u_ref_t, with_slack, verbose); + end + us = [us, u]; + feas = [feas, extra_t.feas]; + comp_times = [comp_times, comp_times]; + 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 + + %% Run simulation for one time step. + t_end_t = min(t + dt, t0+T); + if ~isempty(end_event) + ode_opt = odeset('Events', end_event); + [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 = (ts_t(end) == t0 + T) || ~isempty(t_event); + else + [ts_t, xs_t] = ode_func(@(t, x) plant_sys.dynamics(t, x, u), ... + [t, t_end_t], x); + end_simulation = ts_t(end) == t0 + T; + end + t = ts_t(end); + x = xs_t(end, :)'; + u_prev = u; + + %% Record traces. + xs = [xs, x]; + ts = [ts, t]; +end % end of the main while loop +%% Add control input for the final timestep. +if isempty(u_ref) + u_ref_t = ratio_u_diff * u_prev; +else + u_ref_t = u_ref; +end +[u, extra_t] = controller(x, u_ref_t, with_slack, verbose); +us = [us, u]; +feas = [feas, extra_t.feas]; +comp_times = [comp_times, comp_times]; +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 + +%% 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 +end % end of the main function. diff --git a/lib/@CtrlAffineSysFL/rollout_controller_eval_clf_FL.m b/lib/sim/rollout_controller_eval_clf_FL.m similarity index 100% rename from lib/@CtrlAffineSysFL/rollout_controller_eval_clf_FL.m rename to lib/sim/rollout_controller_eval_clf_FL.m 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 From 46f655429bd6a151bff673a72e05ca557560d7fb Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Tue, 5 Oct 2021 17:34:10 +0900 Subject: [PATCH 14/49] 1. rollout_controller_with_contact.m added This function supports the dynamics that encompass reset map function. Issue) Should reset map, event_option be included in varargin (I think not!) You have to define reset_map_function, event_option to rollout this function 2. run_clf_simulation_rabbit.m refined It's pipeline got much simpler! 1) Prepare the simulation config and dynamics, event_handler 2) Initialize the state 3) Run simulation 4) Visualize the key plots 5) Run the animation --- demos/rabbit/plot_rabbit_result.m | 8 +- .../rabbit/rollout_controller_with_contact.m | 157 ++++++++++++++++++ demos/rabbit/run_clf_simulation_rabbit.m | 60 ++----- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 22 +-- dynsys/@RabbitBuiltIn/reset_map.m | 6 + 5 files changed, 189 insertions(+), 64 deletions(-) create mode 100644 demos/rabbit/rollout_controller_with_contact.m create mode 100644 dynsys/@RabbitBuiltIn/reset_map.m diff --git a/demos/rabbit/plot_rabbit_result.m b/demos/rabbit/plot_rabbit_result.m index e01eca0..5186c3e 100644 --- a/demos/rabbit/plot_rabbit_result.m +++ b/demos/rabbit/plot_rabbit_result.m @@ -10,11 +10,11 @@ function plot_rabbit_result(xs, ts, us, extras) Vs = []; dVs_error = []; etas_zs = []; feas = []; slacks = []; mus = []; ys = []; dys = []; Fsts = []; for i = 1:length(extras) - Vs = [Vs; extras(i).training_datas.Vs(1:end-1)]; + Vs = [Vs; extras(i).training_datas.Vs]; dVs_error = [dVs_error; extras(i).training_datas.dVs_error']; - etas_zs = [etas_zs; extras(i).training_datas.etas_zs(1:end-1, :)]; - dys = [dys; extras(i).dys(1:end-1, :)]; - ys = [ys; extras(i).ys(1:end-1, :)]; + etas_zs = [etas_zs; extras(i).training_datas.etas_zs]; + dys = [dys; extras(i).dys]; + ys = [ys; extras(i).ys]; feas = [feas; extras(i).feas]; slacks = [slacks; extras(i).slacks]; diff --git a/demos/rabbit/rollout_controller_with_contact.m b/demos/rabbit/rollout_controller_with_contact.m new file mode 100644 index 0000000..0a0a30a --- /dev/null +++ b/demos/rabbit/rollout_controller_with_contact.m @@ -0,0 +1,157 @@ +function [xs, us, ts, extras] = rollout_controller_with_contact(... + x0, plant_sys, control_sys, controller, dt, ... + event_options, reset_map_function, nstep, ... + varargin) +% TODO: to generalize this concept, maybe hybrid simulation might +% encompass well +% Input +% Necessary +% nstep: number of contacts +% event: function handle that detect the contact +% reset_map_function: reset the state after detection of the contact +% Default +% varargin: with_slack, mu0, verbose, event_options +% +% Output: +% xs: trajectory +% us: control inputs +% mus: virtual control inputs +% ts: time stamps +% etas_zs: [y dy phase dphase] +% feas: socp feasibility +% slacks: slack + + settings = parse_function_args(varargin{:}); + + if ~isfield(settings, 'with_slack') + with_slack = 0; + else + with_slack = settings.with_slack; + end + + if ~isfield(settings, 'mu0') + mu0 = []; + else + mu0 = settings.mu0; + end + + if ~isfield(settings, 'verbose') + verbose = 0; + else + verbose = settings.verbose; + end + + %% Record Paper + x = x0; + t0 = 0; % TODO: t0 in varargin(?) + % initialize traces. + xs = []; + etas_zs = []; + ts = []; + Vs = []; + us = []; + mus = []; + Fsts = []; + ys = []; + dys = []; + dVs_error = []; + feas = []; + slacks = []; + step_logs = []; + + %% Run Step Simulation + for k = 1:nstep + step_summary = strcat("[Step]", num2str(k)); disp(step_summary); + + % Initialize all robot status each step + is_walking = false; + + % change this to lyapunov controller + [xs_new, us_new, ts_new, extras_new] = rollout_controller_eval_clf_FL( ... + x0, t0, plant_sys, control_sys, controller, dt, ... + 'with_slack', with_slack, 'verbose', verbose, 'event_options', event_options, 'mu0', mu0); + + Vs_new = extras_new.training_datas.Vs; + dVs_error_new = extras_new.training_datas.dVs_error; + etas_zs_new = extras_new.training_datas.etas_zs; + + ys_new = extras_new.ys; + dys_new = extras_new.dys; + Fsts_new = extras_new.Fsts; + mus_new = extras_new.mus; + feas_k = extras_new.feas; + slacks_k = extras_new.slacks; + + % TODO: Reset Map when event is detected, no uncertainty term related + x0 = reset_map_function(xs_new(end, :)); + t0 = ts_new(end); + + % If robot has fallen, display the robot's status and stop + % collecting datas + height_threshold = control_sys.params.fall_threshold; % threshold to judge if robot has fallen. + height_trajectory = xs_new(:, 2); + last_valid = find(height_trajectory Date: Fri, 8 Oct 2021 01:04:10 -0700 Subject: [PATCH 15/49] modify function headers and support vectorized input. --- lib/@CtrlAffineSys/CtrlAffineSys.m | 82 ++++++++++++++++++++++-------- lib/@CtrlAffineSys/ctrlCbfClfQp.m | 29 ++++++----- lib/@CtrlAffineSys/ctrlCbfQp.m | 26 +++++----- lib/@CtrlAffineSys/ctrlClfQp.m | 28 +++++----- lib/@CtrlAffineSys/init_sys.m | 4 +- 5 files changed, 107 insertions(+), 62 deletions(-) diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 178c45b..631b89e 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -110,7 +110,9 @@ % 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 @@ -119,6 +121,7 @@ % 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 @@ -130,6 +133,7 @@ % 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 @@ -140,14 +144,16 @@ % Vs = obj.clf(x) % For 'built-in' setup, override this function with the % user-defined implementation of the Control Lyapunov Function V(x). - % Vs: (size: (obj.n_clf, 1)) + % 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 - Vs = 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); - Vs(i) = clf_i; + Vs(i, :) = clf_i; end end @@ -155,14 +161,16 @@ % 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)}. - % LfVs: (size: (obj.n_clf, 1)) + % 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 - LfVs = 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); - LfVs(i) = lf_clf_i; + LfVs(i, :) = lf_clf_i; end end @@ -170,29 +178,46 @@ % 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)}. - % LgVs: (size: (obj.n_clf, obj.udim)) + % 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 - LgVs = 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); - LgVs(i, :) = lg_clf_i; + LgVs(i, :, :) = lg_clf_i; end end + 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). - % Bs: (size: (obj.n_cbf, 1)) + % 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 - Bs = 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); - Bs(i) = cbf_i; + Bs(i, :) = cbf_i; end end @@ -200,14 +225,16 @@ % 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)}. - % LfBs: (size: (obj.n_cbf, 1)) + % 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 - LfBs = 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); - LfBs(i) = lf_cbf_i; + LfBs(i, :) = lf_cbf_i; end end @@ -215,16 +242,31 @@ % 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)}. - % LgBs: (size: (obj.n_cbf, obj.udim)) + % 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 - LgBs = 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); - LgBs(i, :) = lg_cbf_i; + 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 end end diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index ba4e7d9..bb96a50 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -1,18 +1,19 @@ -%% Author: Jason Choi (jason.choi@berkeley.edu) function [u, extraout] = ctrlCbfClfQp(obj, x, u_ref, with_slack, verbose) - %% Implementation of the 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 - % 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. +%% [u, extraout] = ctrlCbfClfQp(obj, x, u_ref, with_slack, verbose) +%% Implementation of the 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 +% 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 diff --git a/lib/@CtrlAffineSys/ctrlCbfQp.m b/lib/@CtrlAffineSys/ctrlCbfQp.m index 6984167..ff75e66 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -1,16 +1,18 @@ -%% Author: Jason Choi (jason.choi@berkeley.edu) function [u, extraout] = 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 - % 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. +%% [u, extraout] = 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 +% 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 diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index f8c068c..a36e46f 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -1,18 +1,18 @@ -%% Author: Jason Choi (jason.choi@berkeley.edu) function [u, extraout] = ctrlClfQp(obj, x, u_ref, with_slack, verbose) - %% Implementation of the 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 - % 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. - +%% [u, extraout] = ctrlClfQp(obj, x, u_ref, with_slack, verbose) +%% Implementation of the 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 +% 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 diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 56d7923..80fdc8e 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -144,7 +144,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) @@ -157,7 +157,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 % what? + elseif length(params.u_max) ~= obj.udim error("Invalid size of params.u_max."); else if isrow(params.u_max) From ae4b6bbd96f6571ab254e7a49afd74378ef6b93f Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Fri, 8 Oct 2021 22:34:18 +0900 Subject: [PATCH 16/49] version compatiblize Signed-off-by: goldenhazard --- demos/rabbit/rollout_controller_with_contact.m | 4 ++-- demos/rabbit/run_clf_simulation_rabbit.m | 14 +++++++------- lib/sim/rollout_controller_eval_clf_FL.m | 1 + 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/demos/rabbit/rollout_controller_with_contact.m b/demos/rabbit/rollout_controller_with_contact.m index 0a0a30a..f64055e 100644 --- a/demos/rabbit/rollout_controller_with_contact.m +++ b/demos/rabbit/rollout_controller_with_contact.m @@ -6,9 +6,9 @@ % encompass well % Input % Necessary -% nstep: number of contacts -% event: function handle that detect the contact +% event_options: function handle that detect the contact % reset_map_function: reset the state after detection of the contact +% nstep: number of contacts % Default % varargin: with_slack, mu0, verbose, event_options % diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 56705ee..dd5f435 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -8,7 +8,7 @@ % This is only designed for enabling animation of bipedal walker global animation_scale -%% Set Parameters +%% Step 1. Set Simulation Setting and System-parameters % Simulation Setting % sim_t: overall simulation time: do not need it in this mode actually % dt: time interval to collect data used for plot/animation @@ -24,11 +24,11 @@ with_slack = true; % use slack variable in QP (change this into false in order not to use it) verbose = false; % show the QP-solving process -%% Prepare System (Uncertainty control) +%% Step 2. Prepare System (Uncertainty control) control_sys = RabbitBuiltIn(params); plant_sys = RabbitBuiltIn(params); -% Reflect model uncertainty here +%% Reflect model uncertainty here plant_sys.params.scale = 1.5; %plant_sys.params.torso_add = 10; @@ -36,7 +36,7 @@ event_options = odeset('Events', @plant_sys.rabbit_event); % contact detector reset_map_function = @plant_sys.reset_map; % reset mapper -%% Initialize state +%% Step 3. Initialize state % Init state. q0_ini = params.legacy.q0_ini; dq0_ini = params.legacy.dq0_ini; @@ -44,18 +44,18 @@ x = x0; t0 = 0; -%% Main Simulation +%% Step 4. Main Simulation % rollout_controller_with_contact_version(for hybrid system) [xs, us, ts, extras] = rollout_controller_with_contact(... x0, plant_sys, control_sys, controller, dt, ... event_options, reset_map_function, nstep,... 'with_slack', with_slack, 'verbose', verbose); -%% Plot the result +%% Step 5. Plot the result % TODO: tune the plot setting into new version plot_rabbit_result(xs, ts, us, extras); -%% Five Link Animation +%% Step 6. Five Link Animation animation_scale = plant_sys.params.scale; animation_dt = 0.05; x_quan_vec = coordinateTransformation(xs); diff --git a/lib/sim/rollout_controller_eval_clf_FL.m b/lib/sim/rollout_controller_eval_clf_FL.m index b69a434..f3f7598 100644 --- a/lib/sim/rollout_controller_eval_clf_FL.m +++ b/lib/sim/rollout_controller_eval_clf_FL.m @@ -11,6 +11,7 @@ % sim_t = simulation time settings = parse_function_args(varargin{:}); + if ~isfield(settings, 'with_slack') with_slack = 0; else From 6dc074b0d9579d7ccc4c852e12a61f023f3b9c71 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Mon, 11 Oct 2021 01:03:41 +0900 Subject: [PATCH 17/49] Rollout_controller with feedback linearized version updated Below updates are designed to use 'rollout_controller' for all controllers. (1) CtrlAffineSysFL/wrap_controller.m This makes controller to be compatible with rollout_controller function. Every control system that has different notion of the basic controller function should have this function to make the form compatible with the rollout_controller function (2) Suggestion We lose certain information that requires the system to evolve. ex) dVs_error, ... Those kind of information should be parsed after the evolution of the system. I suggest to use the function like 'evolve_System' instead of explicit use of ode_func By this, we can use same notion of 'wrap_evolve_system' to parse various information regardless of the control system's format This kind of update would make the code more robust as far as I am concerned. (3) version that supports both mu_ref and u_ref should be updated in future. --- demos/rabbit/plot_rabbit_result.m | 6 +- ..._controller_for_multiple_resets_complex.m} | 27 ++- ...ut_controller_for_multiple_resets_simple.m | 132 ++++++++++++++ demos/rabbit/run_clf_simulation_rabbit.m | 39 +++- lib/@CtrlAffineSys/CtrlAffineSys.m | 2 +- lib/@CtrlAffineSysFL/ctrlClfQpFL.m | 10 +- lib/@CtrlAffineSysFL/wrap_controller.m | 32 ++++ lib/sim/rollout_controller.m | 85 +++++++-- lib/sim/rollout_controller_eval_clf_FL.m | 40 +++-- lib/sim/rollout_controller_eval_clf_FL_orig.m | 170 ++++++++++++++++++ 10 files changed, 484 insertions(+), 59 deletions(-) rename demos/rabbit/{rollout_controller_with_contact.m => rollout_controller_for_multiple_resets_complex.m} (84%) create mode 100644 demos/rabbit/rollout_controller_for_multiple_resets_simple.m create mode 100644 lib/@CtrlAffineSysFL/wrap_controller.m create mode 100644 lib/sim/rollout_controller_eval_clf_FL_orig.m diff --git a/demos/rabbit/plot_rabbit_result.m b/demos/rabbit/plot_rabbit_result.m index 5186c3e..4a457dc 100644 --- a/demos/rabbit/plot_rabbit_result.m +++ b/demos/rabbit/plot_rabbit_result.m @@ -10,9 +10,9 @@ function plot_rabbit_result(xs, ts, us, extras) Vs = []; dVs_error = []; etas_zs = []; feas = []; slacks = []; mus = []; ys = []; dys = []; Fsts = []; for i = 1:length(extras) - Vs = [Vs; extras(i).training_datas.Vs]; - dVs_error = [dVs_error; extras(i).training_datas.dVs_error']; - etas_zs = [etas_zs; extras(i).training_datas.etas_zs]; + Vs = [Vs; extras(i).logs.Vs]; + dVs_error = [dVs_error; extras(i).logs.dVs_error']; + etas_zs = [etas_zs; extras(i).logs.etas_zs]; dys = [dys; extras(i).dys]; ys = [ys; extras(i).ys]; diff --git a/demos/rabbit/rollout_controller_with_contact.m b/demos/rabbit/rollout_controller_for_multiple_resets_complex.m similarity index 84% rename from demos/rabbit/rollout_controller_with_contact.m rename to demos/rabbit/rollout_controller_for_multiple_resets_complex.m index f64055e..25c33f8 100644 --- a/demos/rabbit/rollout_controller_with_contact.m +++ b/demos/rabbit/rollout_controller_for_multiple_resets_complex.m @@ -1,16 +1,15 @@ -function [xs, us, ts, extras] = rollout_controller_with_contact(... +function [xs, us, ts, extras] = rollout_controller_for_multiple_resets_complex(... x0, plant_sys, control_sys, controller, dt, ... - event_options, reset_map_function, nstep, ... + event_func, reset_map_function, nstep, ... varargin) -% TODO: to generalize this concept, maybe hybrid simulation might -% encompass well +% Rollout control for the hybrid system that uses reset_map functions % Input % Necessary -% event_options: function handle that detect the contact +% event_func: function handle that detect the contact % reset_map_function: reset the state after detection of the contact % nstep: number of contacts % Default -% varargin: with_slack, mu0, verbose, event_options +% varargin: with_slack, mu0, verbose, event_func % % Output: % xs: trajectory @@ -58,7 +57,6 @@ feas = []; slacks = []; step_logs = []; - %% Run Step Simulation for k = 1:nstep step_summary = strcat("[Step]", num2str(k)); disp(step_summary); @@ -69,11 +67,12 @@ % change this to lyapunov controller [xs_new, us_new, ts_new, extras_new] = rollout_controller_eval_clf_FL( ... x0, t0, plant_sys, control_sys, controller, dt, ... - 'with_slack', with_slack, 'verbose', verbose, 'event_options', event_options, 'mu0', mu0); + 'with_slack', with_slack, 'verbose', verbose, 'event_func', event_func); + % extras_new: y, dy, Fst, mu, - Vs_new = extras_new.training_datas.Vs; - dVs_error_new = extras_new.training_datas.dVs_error; - etas_zs_new = extras_new.training_datas.etas_zs; + Vs_new = extras_new.logs.Vs; + dVs_error_new = extras_new.logs.dVs_error; + etas_zs_new = extras_new.logs.etas_zs; ys_new = extras_new.ys; dys_new = extras_new.dys; @@ -141,9 +140,9 @@ end end - extras.training_datas.Vs = Vs; - extras.training_datas.dVs_error = dVs_error; - extras.training_datas.etas_zs = etas_zs; + extras.logs.Vs = Vs; + extras.logs.dVs_error = dVs_error; + extras.logs.etas_zs = etas_zs; extras.feas = feas; extras.slacks = slacks; diff --git a/demos/rabbit/rollout_controller_for_multiple_resets_simple.m b/demos/rabbit/rollout_controller_for_multiple_resets_simple.m new file mode 100644 index 0000000..b7eebc7 --- /dev/null +++ b/demos/rabbit/rollout_controller_for_multiple_resets_simple.m @@ -0,0 +1,132 @@ +function [xs, us, ts, extras] = rollout_controller_for_multiple_resets_simple(... + x0, plant_sys, control_sys, controller, dt, ... + event_func, reset_map_function, nstep, ... + varargin) +% Rollout control for the hybrid system that uses reset_map functions +% Input +% Necessary +% event_options: function handle that detect the contact +% reset_map_function: reset the state after detection of the contact +% nstep: number of contacts +% Default +% varargin: with_slack, mu0, verbose, event_options +% +% Output: +% xs: trajectory +% us: control inputs +% mus: virtual control inputs +% ts: time stamps +% etas_zs: [y dy phase dphase] +% feas: socp feasibility +% slacks: slack + + settings = parse_function_args(varargin{:}); + + if ~isfield(settings, 'with_slack') + with_slack = 0; + else + with_slack = settings.with_slack; + end + + if ~isfield(settings, 'mu0') + mu0 = []; + else + mu0 = settings.mu0; + end + + if ~isfield(settings, 'mu_ref') + mu_ref = []; + else + mu_ref = settings.mu_ref; + end + + if ~isfield(settings, 'verbose') + verbose = 0; + else + verbose = settings.verbose; + end + + %% Record Paper + sim_t = inf; % set enough time if sim_t is not the standard of the end + x = x0; + t0 = 0; % TODO: t0 in varargin(?) + % initialize traces. + xs = []; + ts = []; + Vs = []; + us = []; + mus = []; + feas = []; + slacks = []; + +% FL_linearizer = @(obj, x, mu) (obj.ctrlFeedbackLinearize(x, mu)); +% FL_logger = @(obj, x, mu) (obj.logRecords(x, mu)); + %% Run Step Simulation + for k = 1:nstep + step_summary = strcat("[Step]", num2str(k)); disp(step_summary); + + % Initialize all robot status each step + is_walking = false; + + [xs_new, us_new, ts_new, extras_new] = rollout_controller( ... + x0, plant_sys, control_sys, controller, sim_t,... + 't0', t0, 'dt', dt, 'end_event', event_func, ..., + 'verbose', verbose, 'with_slack', with_slack); + + Vs_new = extras_new.Vs; + mus_new = extras_new.mus; + feas_k = extras_new.feas; + slacks_k = extras_new.slacks; + + % TODO: Reset Map when event is detected, no uncertainty term related + x0 = reset_map_function(xs_new(:, end)'); + t0 = ts_new(end); + + % If robot has fallen, display the robot's status and stop + % collecting datas + height_threshold = control_sys.params.fall_threshold; % threshold to judge if robot has fallen. + height_trajectory = xs_new(2, :); + last_valid = find(height_trajectory transpose different + % use eval_FL +run_simple = true; +if run_simple + [xs, us, ts, extras] = rollout_controller_for_multiple_resets_simple(... + x0, plant_sys, control_sys, simple_controller, dt, ... + event_func, reset_map_function, nstep,... + 'with_slack', with_slack, 'verbose', verbose); + xs = xs'; + ts = ts'; +else + [xs, us, ts, extras] = rollout_controller_for_multiple_resets_complex(... + x0, plant_sys, control_sys, complex_controller, dt, ... + event_func, reset_map_function, nstep,... + 'with_slack', with_slack, 'verbose', verbose); +end %% Step 5. Plot the result -% TODO: tune the plot setting into new version -plot_rabbit_result(xs, ts, us, extras); +if ~run_simple + plot_rabbit_result(xs, ts, us, extras); +end %% Step 6. Five Link Animation animation_scale = plant_sys.params.scale; diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 178c45b..2a82c0a 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -32,7 +32,7 @@ % (when they exist) is not recommended. % (Always preferred to refer directly to the class properties.) params - config + %config %% Functions generated from symbolic expressions. % (Used when setup_option is 'symbolic'.) diff --git a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m index e344289..175b295 100644 --- a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m +++ b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m @@ -1,5 +1,5 @@ %% Author: Jason Choi (jason.choi@berkeley.edu) -function [mu, slack, V, feas] = ctrlClfQpFL(obj, s, mu_ref, with_slack, verbose) +function [mu, extra_t] = ctrlClfQpFL(obj, s, mu_ref, with_slack, verbose) %% Implementation of CLF-QP under feedback linearization structure. % Inputs: s: state % mu_ref: reference virtual control input @@ -10,7 +10,9 @@ % 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.) + % when qp is infeasible, u is determined from quadprog.) + + % TODO: AffinesystemFL => u_ref, mu_ref (varargin input!) if nargin < 3 mu_ref = zeros(obj.udim, 1); end @@ -136,4 +138,8 @@ end slack = []; end + + extra_t.slack = slack; + extra_t.feas = feas; + extra_t.Vs = V; 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/rollout_controller.m b/lib/sim/rollout_controller.m index 0805499..af0abf9 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -51,21 +51,51 @@ % comp_times: trace of computation time for each timestep (length: total_k) settings = parse_function_args(varargin{:}); +% Feedback-linearizer Determinant +if isa(control_sys, 'CtrlAffineSysFL') + is_FL_system = true; +else + is_FL_system = 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(u0) ~= control_sys.udim + 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') + error("Not supported in the current version."); + elseif 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, 'mu_ref') + mu_ref = []; +else + if isa(settings.mu_ref, 'function_handle') + error("Not supported in the current version."); + elseif 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 ~isfield(settings, 'dt') dt = 0.01; else @@ -84,17 +114,6 @@ ratio_u_diff = settings.ratio_u_diff; end -if ~isfield(settings, 'u_ref') - u_ref = []; -else - if isa(settings.u_ref, 'function_handle') - error("Not supported in the current version."); - elseif 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, 'verbose') verbose = 0; else @@ -137,15 +156,23 @@ slacks = []; comp_times = []; +mus = []; +ys = []; +dys = []; + % Initialize state & time. x = x0; t = t0; + u_prev = zeros(control_sys.udim, 1); +mu_prev = zeros(control_sys.udim, 1); + end_simulation = false; %% Run simulation. % _t indicates variables for the current loop. while ~end_simulation - %% Determin control input. + %% Determine control input. + % TODO: can be simpler, but confusing. if t == t0 && ~isempty(u0) u = u0; else @@ -154,8 +181,22 @@ else u_ref_t = u_ref; end - [u, extra_t] = controller(x, u_ref_t, with_slack, verbose); + if is_FL_system + % TODO: going to support u_ref, mu_ref ver. + %[u, extra_t] = controller(x, u_ref_t, mu_ref_t, with_slack, verbose); + % mu_ref_t should be supported + if isempty(mu_ref) + mu_ref_t = ratio_u_diff * u_prev; + else + mu_ref_t = mu_ref; + end + [u, extra_t] = controller(x, mu_ref_t, with_slack, verbose); + else + [u, extra_t] = controller(x, u_ref_t, with_slack, verbose); + end + end + us = [us, u]; feas = [feas, extra_t.feas]; comp_times = [comp_times, comp_times]; @@ -189,6 +230,11 @@ %% Record traces. xs = [xs, x]; ts = [ts, t]; + if is_FL_system + ys = [ys, extra_t.y]; + dys = [dys, extra_t.dy]; + mus = [mus, extra_t.mu]; + end end % end of the main while loop %% Add control input for the final timestep. if isempty(u_ref) @@ -197,6 +243,7 @@ u_ref_t = u_ref; end [u, extra_t] = controller(x, u_ref_t, with_slack, verbose); + us = [us, u]; feas = [feas, extra_t.feas]; comp_times = [comp_times, comp_times]; @@ -209,6 +256,11 @@ if isfield(extra_t, 'Bs') Bs = [Bs, extra_t.Bs]; end +if is_FL_system + ys = [ys, extra_t.y]; + dys = [dys, extra_t.dy]; + mus = [mus, extra_t.mu]; +end %% Make extraout extraout.feas = feas; @@ -222,4 +274,9 @@ if ~isempty(Bs) extraout.Bs = Bs; end +if is_FL_system + extraout.mus = mus; + extraout.ys = ys; + extraout.dys = dys; +end end % end of the main function. diff --git a/lib/sim/rollout_controller_eval_clf_FL.m b/lib/sim/rollout_controller_eval_clf_FL.m index f3f7598..07925c1 100644 --- a/lib/sim/rollout_controller_eval_clf_FL.m +++ b/lib/sim/rollout_controller_eval_clf_FL.m @@ -1,5 +1,5 @@ % function [xs, us, mus, ts, Vs, dVs_error, etas_zs, feas, slacks, extras] = rollout_controller_eval_clf_FL( ... -% x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_options) +% x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_func) function [xs, us, ts, extras] = rollout_controller_eval_clf_FL( ... x0, t0, plant_sys, control_sys, controller, dt, varargin) %% Parse varargin into settings @@ -7,33 +7,33 @@ % with_slack % mu0 = reference for mu input % verbose = 0(run quitely), 1(show QP info) - % event_options = run simulation until event happens + % event_func = run simulation until event happens % sim_t = simulation time settings = parse_function_args(varargin{:}); -if ~isfield(settings, 'with_slack') - with_slack = 0; -else - with_slack = settings.with_slack; -end - if ~isfield(settings, 'mu0') mu0 = []; else mu0 = settings.mu0; end +if ~isfield(settings, 'with_slack') + with_slack = 0; +else + with_slack = settings.with_slack; +end + if ~isfield(settings, 'verbose') verbose = 0; else verbose = settings.verbose; end -if ~isfield(settings, 'event_options') - event_options = 0; +if ~isfield(settings, 'event_func') + event_func = []; else - event_options = settings.event_options; + event_func = settings.event_func; end if ~isfield(settings, 'sim_t') @@ -90,19 +90,25 @@ slack = 0; else % Determin control input from the controller. - [mu, slack, V, feas_k] = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); % mu + [mu, extra_t] = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); % mu u = control_sys.ctrlFeedbackLinearize(x, mu); % u=u_star + LgLf\mu dV_hat = LfV + LgV * mu; % model based estimate of dV end + + slack = extra_t.slack; + V = extra_t.Vs; + feas_k = extra_t.feas; + feas(k, :) = feas_k; us(k, :) = u; mus(k, :) = mu; slacks(k, :) = slack; % Run simulation for one time step with determined control input. - if isempty(event_options) + if isempty(event_func) [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! else + event_options = odeset('Events', event_func); [ts_k, xs_k, te, xe, ~] = ... ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! end @@ -116,7 +122,7 @@ %% Recording Section: Evaluate CLF related values after one time step. % Record dVs_error(k) = dV - dV_hat % Vs = V_next - [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); + [y_next, dy_next, ~, ~, ~] = control_sys.eval_y(x_next); z_next = control_sys.z(x_next); ys(k+1, :) = y_next; % Wonsuhk_revised dys(k+1, :) = dy_next; @@ -156,9 +162,9 @@ %% Archive extra datas % training data required to train the model (refer to GP-CLF-SOCP ver) -extras.training_datas.Vs = Vs; -extras.training_datas.dVs_error = dVs_error; -extras.training_datas.etas_zs = etas_zs; +extras.logs.Vs = Vs; +extras.logs.dVs_error = dVs_error; +extras.logs.etas_zs = etas_zs; % Tune this in order to retrieve the specific params you want to save extras.feas = feas; diff --git a/lib/sim/rollout_controller_eval_clf_FL_orig.m b/lib/sim/rollout_controller_eval_clf_FL_orig.m new file mode 100644 index 0000000..1801182 --- /dev/null +++ b/lib/sim/rollout_controller_eval_clf_FL_orig.m @@ -0,0 +1,170 @@ +function [xs, us, ts, extras] = rollout_controller_eval_clf_FL_orig( ... + x0, t0, plant_sys, control_sys, controller, dt, varargin) +%% Parse varargin into settings +% Support following symbols for varargin + % with_slack + % mu0 = reference for mu input + % verbose = 0(run quitely), 1(show QP info) + % event_options = run simulation until event happens + % sim_t = simulation time + +settings = parse_function_args(varargin{:}); +if ~isfield(settings, 'with_slack') + with_slack = 0; +else + with_slack = settings.with_slack; +end + +if ~isfield(settings, 'mu0') + mu0 = []; +else + mu0 = settings.mu0; +end + +if ~isfield(settings, 'verbose') + verbose = 0; +else + verbose = settings.verbose; +end + +if ~isfield(settings, 'event_func') + event_func = []; +else + event_func = settings.event_func; +end + +if ~isfield(settings, 'sim_t') + % the dynamics that engage reset map should not be judged by sim_t + % set sim_t to enough time + sim_t = 100; +else + sim_t = settings.sim_t; +end + +%% Prepare record paper +% Total time step +total_k = ceil(sim_t / dt); + +% Initialize traces. +xs = zeros(1, plant_sys.xdim); % trace of the simulation. +ys = zeros(1, control_sys.ydim); % Wonsuhk Revised +ts = zeros(2, 1); +Vs = zeros(2, 1); +dys = zeros(1, control_sys.ydim); % Wonsuhk Revised +us = zeros(1, control_sys.udim); +mus = zeros(1, control_sys.udim); +dVs_error = zeros(1, 1); +feas = false(1, 1); % added +slacks = zeros(1, 1); % added +F_sts = zeros(1, 2); % added + +x = x0; xs(1, :) = x0; +t = t0; ts(1) = t; + +[y, dy, ~, ~, ~] = control_sys.eval_y(x); + +z = control_sys.z(x); +ys(1, :) = y; +dys(1, :) = dy; +Vs(1) = control_sys.clf_FL(y, dy); +zs(1, :) = z; +%% Run Simulation + +for k = 1:total_k-1 + % Evaluate CLf related values + z = control_sys.z(x); + V = control_sys.clf_FL(y, dy); + LfV = control_sys.lF_clf_FL(y, dy); + LgV = control_sys.lG_clf_FL(y, dy); + + %% Determine control input + if k ==1 && ~isempty(mu0) + % Apply predetermined control input for the initial step. + mu = mu0; %mu0 + u = control_sys.ctrlFeedbackLinearize(x, mu); + dV_hat = LfV + LgV * mu; % model based estimate of dV + feas_k = true; + slack = 0; + else + % Determin control input from the controller. + [mu, extra_t] = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); % mu + u = control_sys.ctrlFeedbackLinearize(x, mu); % u=u_star + LgLf\mu + dV_hat = LfV + LgV * mu; % model based estimate of dV + end + feas_k = extra_t.feas; + slack = extra_t.slack; + V = extra_t.Vs; + feas(k, :) = feas_k; + us(k, :) = u; + mus(k, :) = mu; + slacks(k, :) = slack; + + % Run simulation for one time step with determined control input. + if isempty(event_func) + [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! + else + event_options = odeset('Events', event_func); + [ts_k, xs_k, te, xe, ~] = ... + ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! + end + + t = ts_k(end); + ts(k+1) = t; + x_next = xs_k(end, :)'; + % x_next = clip_theta(xs_k(end, :))'; + xs(k+1, :) = x_next; + + %% Recording Section: Evaluate CLF related values after one time step. + % Record dVs_error(k) = dV - dV_hat + % Vs = V_next + [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); + z_next = control_sys.z(x_next); + ys(k+1, :) = y_next; % Wonsuhk_revised + dys(k+1, :) = dy_next; + V_next = control_sys.clf_FL(y_next, dy_next); + LfV_next = control_sys.lF_clf_FL(y_next, dy_next); + LgV_next = control_sys.lG_clf_FL(y_next, dy_next); + % Estimating Vdot based on second order numerical differentiation. + dV_hat_next = LfV_next + LgV_next * mu; + dV_hat = (dV_hat + dV_hat_next)/2; + % Estimation of actual Vdot based on numerical differentiation + dV = (V_next - V) / dt; + dVs_error(k) = dV - dV_hat; + Vs(k+1) = V_next; + zs(k+1, :) = z_next; + + [F_st_u_next, F_st_nu_next] = plant_sys.get_force(x_next); + F_st_next = F_st_u_next*u + F_st_nu_next; + F_sts(k, :) = F_st_next'; + + if verbose + fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... + [dV, dV_hat, dV-dV_hat, LfV, x(3), x(4)]); + end + + if ~isempty(te), break; end + + x = x_next; + y = y_next; + dy = dy_next; + % phase = phase_next; + +end % end of main for loop + +% Cache the training data in aggregated form +etas = [ys,dys]; +etas_zs = [etas zs]; + +%% Archive extra datas +% training data required to train the model (refer to GP-CLF-SOCP ver) +extras.logs.Vs = Vs; +extras.logs.dVs_error = dVs_error; +extras.logs.etas_zs = etas_zs; + +% Tune this in order to retrieve the specific params you want to save +extras.feas = feas; +extras.slacks = slacks; +extras.mus = mus; +extras.ys = ys; +extras.dys = dys; +extras.Fsts = F_sts; \ No newline at end of file From a3318ea678f6eacaed19abb5c342ea4a09791444 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 12 Oct 2021 01:46:00 -0700 Subject: [PATCH 18/49] change CtrlAffineSys controller function interface. --- lib/@CtrlAffineSys/CtrlAffineSys.m | 1 - lib/@CtrlAffineSys/ctrlCbfClfQp.m | 24 ++++++++++----- lib/@CtrlAffineSys/ctrlCbfQp.m | 27 ++++++++++++----- lib/@CtrlAffineSys/ctrlClfQp.m | 22 +++++++++----- lib/sim/rollout_controller.m | 47 +++++++++++++++++++++++------- 5 files changed, 87 insertions(+), 34 deletions(-) diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 3b7992b..85ab5ac 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -32,7 +32,6 @@ % (when they exist) is not recommended. % (Always preferred to refer directly to the class properties.) params - %config %% Functions generated from symbolic expressions. % (Used when setup_option is 'symbolic'.) diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index bb96a50..15b9db9 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -1,7 +1,8 @@ -function [u, extraout] = ctrlCbfClfQp(obj, x, u_ref, with_slack, verbose) -%% [u, extraout] = ctrlCbfClfQp(obj, x, u_ref, with_slack, verbose) +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) @@ -21,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; + verbose = 1; + else + verbose = kwargs.verbose; end if size(u_ref, 1) ~= obj.udim diff --git a/lib/@CtrlAffineSys/ctrlCbfQp.m b/lib/@CtrlAffineSys/ctrlCbfQp.m index ff75e66..89b2c59 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -1,7 +1,8 @@ -function [u, extraout] = ctrlCbfQp(obj, x, u_ref, verbose, with_slack) -%% [u, extraout] = ctrlCbfQp(obj, x, u_ref, verbose, with_slack) +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 @@ -17,15 +18,25 @@ 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 nargin < 4 - % Run QP without log in default condition. - verbose = 0; + if ~isfield(kwargs, 'with_slack') + % Relaxing is activated in default condition. + with_slack = 1; + else + with_slack = kwargs.with_slack; end - if nargin < 5 - with_slack = obj.n_cbf > 1; + if ~isfield(kwargs, 'verbose') + % Run QP without log in default condition. + verbose = 1; + else + verbose = kwargs.verbose; end if size(u_ref, 1) ~= obj.udim diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index a36e46f..3a8dc9a 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -1,7 +1,8 @@ -function [u, extraout] = ctrlClfQp(obj, x, u_ref, with_slack, verbose) -%% [u, extraout] = ctrlClfQp(obj, x, u_ref, with_slack, verbose) +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) @@ -16,19 +17,26 @@ 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; + verbose = 1; + else + verbose = kwargs.verbose; end if size(u_ref, 1) ~= obj.udim diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index af0abf9..46c8a30 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -35,7 +35,11 @@ % u_ref: reference signal of u. It can be a constant vector or a function % handle that takes x as input. % DEBUG: -% verbose: print log if 1 +% 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)) @@ -114,10 +118,14 @@ ratio_u_diff = settings.ratio_u_diff; end -if ~isfield(settings, 'verbose') - verbose = 0; +if ~isfield(settings, 'verbose_level') + if isfield(settings, 'verbose') + verbose_level = settings.verbose; + else + verbose_level = 0; + end else - verbose = settings.verbose; + verbose_level = settings.verbose_level; end if ~isfield(settings, 'end_event') @@ -190,13 +198,16 @@ else mu_ref_t = mu_ref; end - [u, extra_t] = controller(x, mu_ref_t, with_slack, verbose); + [u, extra_t] = controller(x, mu_ref_t, with_slack, verbose); else - [u, extra_t] = controller(x, u_ref_t, with_slack, verbose); - end - + [u, extra_t] = controller(x, 'u_ref', u_ref_t, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); + end end - + if verbose_level >= 1 + print_log(t, x, u, extra_t); + end + us = [us, u]; feas = [feas, extra_t.feas]; comp_times = [comp_times, comp_times]; @@ -222,7 +233,7 @@ [ts_t, xs_t] = ode_func(@(t, x) plant_sys.dynamics(t, x, u), ... [t, t_end_t], x); end_simulation = ts_t(end) == t0 + T; - end + end t = ts_t(end); x = xs_t(end, :)'; u_prev = u; @@ -242,7 +253,11 @@ else u_ref_t = u_ref; end -[u, extra_t] = controller(x, u_ref_t, with_slack, verbose); +[u, extra_t] = controller(x, 'u_ref', u_ref_t, ... + 'with_slack', with_slack, 'verbose', (verbose_level>=2)); +if verbose_level >= 1 + print_log(t, x, u, extra_t); +end us = [us, u]; feas = [feas, extra_t.feas]; @@ -280,3 +295,13 @@ extraout.dys = dys; end end % end of the main function. + +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); + % Add custom log here. + fprintf("\n"); +end From 8d3789814f32ada03044105782ce520ccc08cd02 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 12 Oct 2021 04:24:38 -0700 Subject: [PATCH 19/49] add macOS support for complied rabbit-helper functions. --- .../Rabbit_helper/Ce_five_link_walker.mexmaci64 | Bin 0 -> 25368 bytes .../Rabbit_helper/De_five_link_walker.mexmaci64 | Bin 0 -> 13080 bytes .../Rabbit_helper/Ge_five_link_walker.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/J_LeftToe.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/J_RightToe.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/dJ_LeftToe.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/dJ_RightToe.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/p_LeftToe.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/p_RightToe.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/p_Torso.mexmaci64 | Bin 0 -> 9000 bytes .../Rabbit_helper/p_q2_left.mexmaci64 | Bin 0 -> 8984 bytes .../Rabbit_helper/p_q2_right.mexmaci64 | Bin 0 -> 8984 bytes 12 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/dJ_LeftToe.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexmaci64 create mode 100644 demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..f192943f675ea4856ed60cb9832805080d4efd40 GIT binary patch literal 25368 zcmeI5e{kI8edpIs4KBplXB)e5j?&qd^K}<67Em7?sRyZ`#2zB5)HvB4){%@YV+YGp z^aD&&T4l&mxZWn3yV)78cRQnLx>Kfe+tGBYj^`#y)?l#9Phox;N@AH`mSez*3B)l3 z_x1jKo|S}X+WvVnr=8jF_xbU8KF{a(^Ld`{SF5May!GL^iNvB$Boc|Gd;{NuOA?79 zcYQv~H?ugA=;*kqHFuLWee}})(P|>rL6Ok+oY>vbG0?SVU>0^hzR8sLJ@`4ltNtsq z;HnJGt#)+u4-DSYKhrcH-(R+QS5t&gebEdwyhePp^a)?bj;>w3U46jK$MTTZ|m5(dwY-7KdRsV^9Me%p?t?oW7-z zp2ir}=i~d;26Dr+UuitQ!TKRO^ z)g5w9ePu)@Tb^srznw@7HAOh_va~aiSgCsaq&pLd5z2M$S96yxt$#}*@hQrgTM~(B z?&tH3EKu@~VD=OoXwvaxs0rHe17FYs-AGg;xl{~CDv@bvwxtgd(9Vj-qN?F@6I)! z@7vPdbw^L%ZT)Mm>)G0~<+AOKhBaUA>g(T$Q?6<8F2f?clG`j^Yfd!x^==v1v1ZrK zTe|mLw(U;t-u~}^@0vT{WAKaafko&Z?C%qHeYak6$z_+vNd$lO9C{XQTSOTBD7>?L z+3(M#>pw9@@;cq=_MeUQ1bSKcEJ$EM0t*sYkidcj79_ABfdvUHNMJz%|9?ne)B5{f zZ~02gms@Vyxc-}OjNy_QEUfO$|@H}RR7me?Q@DwI7KJb$b4tH8%2zMM{V~#^aE|M zzpu;Xho+ve{&UvfcVOySD?W0nB^kaDP9;;Vhpo6$*FQ+pP)eLVdv@wOfT?$6doo*^ z{KeU`r;?#)J%h2QF4*Z0t*79vivC|vx$v)r6u75#Ilevlm1k&ugWgvJc8Jl(Xqcek zSv5QkvDIcsqVXsU;+?DicUe178C6G+>8SIbftAxe=3T zejmsV)AkPIE7NCu#FIcj1=53byhul>`2!<#KkW}fLNV=xK1JX7B!k53K_8GU1$8pj z!$2=7{ud-D-hXZwo~8}L~P0{O9fKRmLpAc-jxR&W7`8O(LNonMdht zlLX4fVWe9oh{)7gN~x!N=miLm|kQCOw%ppLF2{IG)Q?A1v{Y&Vk-k@GCCb{c zi8B8jX!rAl>uhDnpz;qvk60=;iBKd^lUvBj1znWM!yGc9YrHwuP7Cv42u3RTDa|nH z*!D5zU_%CvfErn(B0&YSB14!gAm>y$xDqUmKzOU<5-VZZ`~TUrvQMHdP78fYvY<$@ zU3J0Yv$<(i z*Z#H7`GQ)X*X3D(PO0=Z1lL9&1C*~q1y+?Kc%{1Tg>rMDc2}qkR$O{t!#>pI17Kg! z>~BLyw$|x@6-g7gB~Pyvr(coD$p{`YZG2SW7zRG;TKo~^%CH2|Do=uSVnZk@fPi62 zj1(sX%FH4?cM>wY1y~qw#J=$aG{`{|?Q@t~iV^_jv}l6Ja<8MS$=N{ry6NBS2(*f_ z(`u>qiVj*N!#5zzqbt6nUbiuN--oT5Y#6AMK-Fv-%xhA^lR0_X_yblh7^~I}BV(P8 zzo6){Z2EUi)nWBX72y7DaZjkuRHvQX#|8Exy-ZT-bsh)h!cPK(8p7)R8p?9Q_d{9C z10wVpA$tyFsqRq;MBa*H;KdLx&36w``yrGbp;8$VVyy8DjdIzS1*TrJ$e)8TJ3cHW zgL>UNSDLkc<$VB82=JtA3JGRwO=1qSII0oVVB{)PWvl5px@J5yIK|TtnN*w1;|XC^ z(wUJ@69z)-h<7MHnCG?1A8O4^!(EZ)QIS+`c=#DjZkGqeyIGBhBOYX`?M}{qji@AC z4SCu1nCzDcqI-MvFgo!HL66Be<)Dv%wIJf$bP+7eU9@AC#RDR_t`VmJ3|;q23sO>f zMAH#;5EqU^r0|ANopx591!(mtj>-Yj`Xa!B0UFhC^%|{+$TXNk9H~~-)cU%JdQJRx zDLho0%`Ee)9s`|3-+tPpRSZsp$S>Dj=1iM2S8t9mO*Ym|R%H_&v~40NXk~U8sr4~w z{vV|lNg!cMXTB`Wi0MZKf;Ee0Dvg|bN(nabpu2EM|Ib6gfu zoEgFc-eEK-1hc9l=J6qkkExJ6VTP8fJX$SsoX4bvlDR1od`!)7oF|pZ)OuYSKh&V& zunYjz;8^y>DzfFYNQ6n5n%l1pFKBorxXCDaiDlL}P?55Q%|l%7l9L`00p{|Wn=~q{ zU3R%CS;>|VDTas5@=?o6SeR29UQo(XmR$*SL?zHL{-U0!-gJL%VtY$-x1`y2zzW)iec7r(DVrTCFWuoQ>K(t0-~e{ zwS>)L?&a7>mXfJN&qI?rTNj5&E(I(GM?HrvB}52wDZuds8(Miymw(bla?Sq`WYu1I zjL6qTGS5A(i!xkEu^4l;Y4A)l!I-%Mpj1f9D7f>xoGVQ(S9l5KvAQe`h7DIMWs>_*zIbDZxn5q2gWA<^+Y{i)iF1>(oMESdzHDX0%3#`E^itHV_3a}AYzk8`U!@~fSWaI!@uO6l93ShV3YH#92U$z$H#xPnnYeSS83Ec#Cr}0em%NM;vNC%m>^Z$s}P}V4HHpBb%O_y=)=-Ej*(o^^SiP<6B6)G z3H}(};}6IdxQ}WK_aVDGK^A`y-mnW#`2h=Ewl*pw1;4Gb7B!w=29`x;D5u2**xiSS zO}1W-?TH3!>oArP1`iABZAC9hWjweXj|s6hR`_>L%K%X&w8`Ksby6!UV#ZD))F|7u zltQ7wJG0e|q*;c2iYb(Wlslci1DZ|=a%wE&dbGn3plZEn8@#gz zB#4)~Ek48UzM;!D_lf{Towv|Ubj6Wekd2iLdc%^x_#N{lBXhz@8@F(f%>P_kSD9(6 z77Mf)AwtJ+V1bdoeInL!_c{n@P-dmjUQ<})ZietYBg;+B-kd<+MiNG`@5-DI#fzEJXXlq6NQXsEo+-;O7vpbZB~w9Wo0Jo0x#b86>Y!SQU)p zRCUy)A&0Rej$yeGXgNmcM{YOKF$XQ8Q=o-54s#;IyR2rg1y8n8lx}$`E9u zM`fdi&yqmVw&YS~VTNpR#A&O!C{nf2C=%`LmGq{{LqbDrpLUK92&Ilc^6vqXlWw#! zTeRP=7Hw%UHqr=}_`1ETOyUQ`kq%1ooKBj*ph)2DK*EuS)&CnBw$~cANy)is(BaRF0kolUk4;yL5%6fkLX2RzU5IHoBux@k zw%Vj%1grAAu_j>^%i^n3bg*lh`j5t!1<=#jL2<$YU|@H^F~ZmA1-cgff_-MHTyF%p z37Hn=VZm5K+hxVbhme&jHMtH}-RdYXb&dz%x%&Yr<%b%mBZgYq2(dUOw}8Q1MXQxb zl?uTPEx3Ylzf39Z!oiU8mTA$}*ky!bZ{$%y$T`JxXRYGX!c~_%eWt7t894lgOa9AZB^}NIPrvO)noS;{zbH1v!tRc`?R>839 zzg=W9wzZ?#V?%i|>hF-0r5rYQN+p?AZmiw8NR6c<;qq+)U8`V3QAzfmWoOz zW0<+p>@3*w19d1TRNXIxA=KJUlwrkpe%pxBy8VRs*AiS?Z%R{)53TnzTH63cia|U3 z)YiIjCZOcj7F*}uj~)HY%~HNh&7CqgCFD~;N3YD)f)L0j>2odoq#_2w4_X4Y4*enSi5&h9AwUnu& zIO5tf)9N+$m9%NK38hhF#TcDtWlD{x>06 zkwk|sMU7YK%e|?2Z68<{Yf$qhKzZBSydlGWi<#7vldtAp7Wg{?s|2xvuCV(2+Kp)o z3vW-1O&~7pK&w8d(`o&ggIip{Rumx@GZfsIb>5u_|+n=NvC68rYJhr zY^t$HRhG3uH(@xGdR;VP-^6E2vptVfAWd1n@fgR;jK(LGM52%*^pu9?V36Db(;}Y& zTO&`c1t4hCVa!$vMC?%Zh)qgu$VntP=K!VBY#r5FojZHkRM8pGqmq=i0>MrTxi7o> zH!PUiab&4UT4yeJR?4WIT`EqKE7sMCA^VDqobB1vU87RF@LH5)P&vr#bVSog+Kt6v zk2dXjG@X{tjVa;Ke30x<7e)7`VJ>AY$Iw^t&6$ooLTS>S!VYR{jb)q%5BBf@h*U*1 z3bNCa`pU42Mu~(So`adk>O4R4c1!lBC1Bi8Nk9Fzqw;cC-F<;cr;!A)ma!x9RhF=n z(`K!z)JOLTv*UtbTJMFZB|{7bacRQ%x21)gAXgi9)=HBMp#MdS4ow0=k}T?eTtUxR z+SsuLhD?Ms&w{!=@)=KyY}9c~aZsnLF(tyniV~Vw8ZweX$+rAT7;!w&4Wd++WiwEl zgD5{0kScU8SRlIRsv|-|3$|voBdknm+G(~`!}boY6OO7NMTr48 z1;dUr-&N45O_*vuP4@?SepqaZ0Lw{8>K@V{JSMFEsyIV)a_8!h%Z9W71K#Tfs&fZ> zV9FgH$pjF{cwMT89N}1Zv2a8W)F^dmDA6$DkS}{ux8`g#bT%4@xi%tpkh{Zo<|yowat+mt0OJnE$anE!K-e4#)wo9$@gKFUeakj;LDqKHOeGDcsfs zB96S5V<-;jmJqJX!1?lNdWyq7WFJHHWHx_FG8uC_SY8`ZFBZ))sLMD=J~WVW1v`dA zl-k~x+Qm7pG-$^Jq!+wLg4_;L;4LCjUMUoLRfZhUf zRbehu?)83u?4WtZb(XDubJ;&#(Qw)P`OIeLhERs<`5B`0v6Kd_;IPUHVtU}v5$Sn3 zY+MOITw(!m|8@W?hwIV+$f~uFmzvE|9D|p`#>@-`>`77HhI>E{*EKbULw4%(ff|p; z5V^gf0?Jp#jx46JttvK~Hx$riq1q&Vf%e4@(JHF>K{w*4>ClUAlJ#UkI7k4(vh~YN zEKh?t|GWYc>j|g22$T?&=k*wx^i(aUwQzP9j8JCv)f^>naO?SfE*j57i69Ejl3=M;q!wnL$VUUz=TqzQ%+GAI#*4Oq# z6vh#_WKP)A5F9B}X)>pGs=ORNI_aou)+Y6GxIwC)oiwe;XnV<0T@+f4B}CTBH0sF} zZEYkH?^n%aTJD@NQn$-UG~ym>l6=q}!VCRI@Oup*B<2Xg%i#t|$>1$)vRCynJUx_ga<$NrOm90iE@Z4RC=S?Ne5n@ORgqs z2ULdx`3pdFfG;5w@hJ3{+nrPnXE>PgHQUhJ!YP1ZSFr(jnH3Qn7J;Rv&*y~tN?q)H z0OWWR0VVM?)wQ^jerp z=@|P#63dQW4<+PZbDd$ep>_GVCVMp|7DMCvxl%WV;X|&2<>#P-8^pv)F zBqLTY`x1W#%v{P7!-~hL09%=KO==TtyEu?xq6M7XpWTD&4Z-z#=nJhQLV*F>QzbvN zuyfR4Zy*WOx3OkE8ag)N@gQt^Ktpx9M=ERaToE!yKEU~_vwn=A9?PybokBSxB7uh?iVcC*pw$#~!B}~NDXnW_7@&t?(>o!6U&~*l_y?ox@M3*QOIxmr_ z+$JA7NX;B7$d=v$Ko6}$dDK0|%+kwqOd7Q1QRm#U4ljFHYdD4W;d(tMrGg8kk&whB z*fSPSh~2VhO>U2-=w%A-FUgpGBGN6=v-#tl8?121EwU8QBJz)Qi)eQAW3M(QT*Yt7 zGRM*~Cpgr}80OFcsR=q}T5;0hbPjND{=R%Il$33b#g7V-?a@*+i zgF=p=c%REkwnN`&=E=r{*BhhZL2@iNEEB?X?w`76*k^CMAC5?J!;;7Ou?48rZYjPW z2~CGg=AE($R4mTNEC;2FaFOx~kFri`FpVap`#t**p3bOg$w5+Wy_e~#X?GmbQXT~R z%2=((Hn3V5xW?fOsz}+B5!$r3s3sitB9Rl<268CQgLXtRu-AgWL^z`ni)|jUnh?0q zOuG8*IQL2b$b|5Imr0EkH`=N+QJ4O*>51aBA zKF?1=2&4W&qfluW{t-3ZZ2ICsHzw{3+2ZVFj|TR%RIi(-?iukptL;9;MN*7y5{1ef zb*USX_H8g1Wq&*;0f(p_CHXzOlZU|p7LP7+cib0IE)BU#X0r>S7Xfvj6N#e`C0M3>Gl}A)uXt=*P1j;? z68z9{KG3;wvwqR-6G?@>Igaa?C3OA52^Y$jc~Iek(@r=V!xR{0QxlFvDzU$bVHVpA z-X>{iJjqc0W+79)9bZ@utG>@~ZqT6<8AIyEKkZ24QNG3dM!tG8RO-4D?`g` z^RCk-j<8%JGZS zR+)d%w?)dyJtc1&re|hqd(fXyV(z}je%1UW_>R_8;z^FCl~ZUhWZP4l6VHrurkPWG zG`pY4?2B7YR$D zb%>5gs>CT@S?`mSbwWn3j&UpPt{O!?F>kvW%r8`?9hU_l2~xY&+zH8}*M~@|yp$ru z?H~r^(~N5!rC?&znNlMOWMwyF&i4xo3ojCnjwdW0g>#X-N#|SG;vXQ+K{(+`iwKeL zH!@Px84(tjLvs{qBd0gWQulO%7s>Z>i%QrLiyAcS2qp1y-Df6MiLR&2P5g*_@uR#O z>DtPZx5F`{!1RYSn`?59&o&gNSpUua`pemyTM8{-ZRyzj(1yFuo;!HnhPyvll>Aoq z;j?F{FG?2Av(g9WCcm|U(((8ATgMFG4Ksk3&H!E|zzewyH$M7%-2>;Izj$%tz&ZRf z(Yc!+I=gBIKX`O@)z|F0%dWfa+H2Q7yAIg(cCLxU7fTneqR_JO>?+-dckpU^;`keg zVF$nBl{o&gN}E+Wq|$9F9aQO$RQkS3C6)eOB~FPG$Nxqp3W?(*lvJ4a8H)9IpS;%V zlh^*m|J3KR%~#szl4h^}YOG(->~PZ&?!{}rvHWkpaP*H}yLRty7EgR>?~{%D590W` z-!G_V3*UM0-OTqI`0EjG{mY+P{dbj<$F4o|Ctv&C!2g(}830D7v@o*|md#Jw55}ExYeb_Y4j&JiVpw*1_(s-2?p}8(`>f z>>2pOt}VO1+_iO}r!OJ&>7D)QJNkNd-+Gy;M@c)biQoj{)B4$N1KXSdz zepJ8X7Je6=AHo09*Ov|_z8+RIckpBTJ9lsG>8HD|Yal`E%1<|Uba(A(?dxmnzjed5 zmfhQK;@AD@NdFogd#>y2+A`2}eb3-6ySm!8@VoVUIbn} zci!&5`<=M_6N$--_>ukL|HaA2$|Y0o;&9^&oipI m^R{f;(mQ}@{&Ewe$BfJGHwuzE21Q}kl&lhs=wo(oLiuMRI45ub literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..04a54118bd3a673062878ab1a3baa00f33746954 GIT binary patch literal 13080 zcmeHOdr(x@8NU!wL$cn5EYVD2yX}n4-8=-FFtyuSb}MUOB8ZjfZVE21MMz+G-G{-% zWXKqpl9Ys*lunx&W|--i>2xLz=~Tx*B*7Q>NDa~W#6+w{p%}A4qsABg{m!`;VVz|9 zXJ@*1=HBzY&-Z=jJCD6wKEC$lt!avqbeEzi({Y<|?@m#aCbYaBz^x@Kiq~80au*BI zfYY_B)sYl!Fwns|G$dnGTO4&Wj_MS}7mDBcWRVDxDs0&-_El=+yZJuA`HE1<~t``d_H^S9xpw)d7J|9^WqA zUXgBq_@sCZ!+Og)nU7e!-dty~a{|+B^v7v~o~?aIwJ=9p2qM&5LMmTrt?!`rxitSF5_1P{hC9qlZGiPj3d z6KxOL`Di(ofMm4WiP)dnGRxi1v1#iY!1N|vxfyGY3dyMnAZ zLc#J#O=ewfMSa8EsO60Zd%Wt{7{#sHrWBpk2x;c5R3$k&N$!pEdMKdx3__;KJ^MEt2XXq0Z*c-nX zI)paArH^300Cy5R0`Rq-h?(#%0jsaAKnpXfH2a7&^E!zTdWUVxjt_Y<{D6;n2Zkb` z<+n`DYv~($Nr1hhs(GOqkEdsis%Zs1LyJIGZB6D|V`J5-t)L4mSIZgiz zXf{YdU2S-@mW@$ddj>quCjbgM0qh=COMuv(X{3fWG=lRWOb3Omt+f+fkdZZX01PN? zA_eW|&^=b2t7_YTbW3sV2#Eb8#$d5YfelzcPQnNX9fMQ_(k&c!s@f6gmeQSr0DN6| z>?FUmbJVGNc8sd-?X)v^6Kaq?NS=s-Y(|hWAf4I=7(J}m4e^2=iVcENil{YM@oa+^ z+ez)GxgHt7E-=Qsh>?aaPFAay6O_C$s;0Xdt06*_BAL05*)RKOwMs%SI=BG66+6kn zFqJSHqL(&U1TlS=VZ%Wcbg^&LrKVp*AD)ZkBSO6oDonu3q)RJKz~V(HTCUKqa}Yt+ zJflQ9^gLQl6Z;aZLtuSct-43yX@o*oZ(kLDo@Klu0FqALAq-mD)_GM&I)YB)l<7;DrKSf0xg4`9b zhS>cX^aFf|%=2JgK-lk@^+YoQ&sXe-L%oey4`eVPwx)h2SGBy{gFtRTq~sQ;-CcsJ zy^L{ZqCKb4w#u>Z{RlDJS{vn{Xs2oR8=Cy0S`F^FKxiA|L>MJI*@HvcD(HfZVRSVD zdvXFGE20+E-W{~*f)CnR2DzVB27xu{<#iOjSWduaF)MnAona@JAafC;jFvvyO=n?9 z+Z02Wr~wqEM+BKg93{pe1;HwP!gS!2FwfqE-y48MiN`R>SOEj50M7v2v32NP7!#(I ziKQwgT1o|eDx%Mmz>gzjVWLO$?>P+G$8dTt6J`vCNIx{S0*tGL!0aW$gyk5#JIe5} z9HIX`aP0BN=otL?C)0eBX}JlbHyP@JhoP1XeI@{f;JFUCbx=k&%oLrcl2~*)#4xjB z2aq7oXOL`alalRatP<6N>1Wvs16zy`h$v-G%6Zs>n!Aq`r*L@Scg9(I2`8Ge^qC+kXFFX*DPV193g8nd z7C~tv1rFXyVtF0qI2>pGKZhB8F96`;rjoLSeS*hj)+W19wRVSe7g@5kcS;yZvbj2C zMz#qh`!0cKVJzste&UwZ54U8-AbRcC~2=Qwnpvv_1&=J;KtJ29|q zn{g@xnJ@{k)V4DFy_E?U}diqwy@~y^9wZWl34oI!6Pgmn%KTTZ5k9K zla7JI=s+U-IVmCbe;C7RkbPBzp|x`*X02jQn6pPfgn`53m2w$oH;984wikGVt;NFa zCEF&|yRB4L-QCbIY8vgqDz=qcA-P zAqZ;7hZb$5^w$W?29T@uK2~Rxll(G0#!7FCw$TnCOAM;;%zX>C+&oTt+QkZh>A+Kp zT-|WSY@=Tytsa~uFlyJJ#PM;TqZi$xhOycmVU}j^#d|jPaGj-bkTt+^w=gmf&qHpp z3d|Y7TY74Dh`9tQ&Oi@luTxF?73lAAt|6&ueltk-`Dek>eJibaY9nBmc9FMIBegmU z5;I?!51)e z$mF6EEZa!K9z?-@_Lfc&=`{uI982|-j za*+einf@Xu%=EjFx7I8@t%YTzRo8$dG7aIvAcYq-z*+%H#%+3-v7xl!GS^tKQP2V|}{jm)Iw4zVU8 z0Qj<#rki`9li$~T6b&zc{Sx94C2iv%5aC%2029)D6+qsWU#b`OAFscdli$+JH|$WG zuK(cxpjijS;>1p7fxrrV(+U8N>p$VfJr(2G8pD{u{Rw$Qsc+Vx0-IU7O@RE15HI@m zrUM8~Z_Wcim)|WS08clU0f2ZA4KIK-Iw%wx$ZlzFM3eXGPX1{Bt+T|r(&;VTk-vUy zS~N9({f#8`wT-c{G2lsR(^~?%F-?8#9YDj4c(lojzyJHD-IJWG^kFf|npV1FY|eD# zer!&vc-||XGsM#@p6TLg5zqVaRFuW7vv8$B=u;vhk@?FP_P7&o=LvB= z=!kC(yN3Si1*u2=68s`Z>fP;_Y%iwMkL4I~>Hd9a_$lpZT5 z{z&)^1!1``5c!R-yl%O#G7<JyY~q1_AwgUDs>W+g!aZ)LXaSR_JY=-mcMG!%m`2WFV1& zLQNCB}d11lsp-@uAOL>&Un4=)VV2kU$dB<1NO zX&DrJ8cRR$$!qeWsJ}9T?|YTUs%z_f#;cj~VC@?D-LEq5E+zH=KE{sy9BQ!#8m(x= z8xa+6L5%-Dz}GU0!1b3aj{pZ7Z{p=;m-sCMFN(ugNnE@qqzqsAL4RESO^Ne$jkx%} z82oX(D-P$2;)(oiarpi?{J6x&{iR_d{IH1M1@l{W{DD<`=gn`8aXIh_g>c~e#QmdMo^mHU1H#f;*xD4C4r-$0NgGm Nf#^kaN8O90{0C&h%Rm4C literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..65beb0d890c3e807138b48ec8b1d94d739d49e62 GIT binary patch literal 8984 zcmeHMZ)_CD6`ymA?Lxzzshv7%+-@pSog3mmgfuR}a0@=TOE|}g4fv>?&DpnSpK|}$ z-2*#qS|e~Jt8zs@pwdc-{3TKpMU_7UC6(H6h@@Ca1jI?zx{8tz^5+n#icV@s32^%Xb+YZzUjG>#u=qAS{Y3Gd2x957`b$&yxG^W{kF? z4+NJXSD|*JmKTbp`t@?7L;nywYaBL$-$JEv_MJcb@qKqbkp08v+21!De7bTyY%;0` zm91XMs-%@%=gX^FSMhzA?WI!)jopVzTr?NPzKQB#Ej?p~sij(WCIH1IG#qjoB&rj*}^_NOflj9i`FZkepeOB69#w&yeend}x^9 z@4HHt)HL;bom4F!4@kbZ(A2+G?3BEX9}+!GCyN0oatb87usGjyG9N$kF7)Hp0(}GL zic;kLqSXHmg}w(*V-L?q|2&q}NVCT<+);qxAE+IV@qoyen8{b@FfRi7F2Q+kfIIsr z@ZJz~%m)K1lJ7GN^eLVY7H~($4$IR|p4!bIxdCs-56P-XePH%oLA@TA>aQ22z8hrx z3ZZp0=5^}xQ!g!ag+x6t@mXrlAmjJ1fQ5!bzX*|DrM~|{6Ztb3FM#m^R{Ih4zWqEl z9eK`U3{214@mDfPkcFYcp;C2jW$4f~k97EpYo!vfN1FHx*REAdhd+XLaTd*y zrG-Afr>j?0RIt-{^=+*lI8|EPj>VSNZshGI-frftpSN3i+sWJQXc^l%)%Z^=QRykI zC3$3|1;&e?Vq1(nLfU(z4UjfR+LNTslJ*p7uaY)J+DoL}LD~zXL17oCp^@S!;{pqL zd5eWFZ~5(SsLVU%31L6+&m4<>1@t4*du98PLS#| znq*zumrLkcO!n=M>TONU%+Wg)9ZilHtT&U9Q_=L8oXMH!DM$6;TuMuu#VTD03X$N#Q$E>@!f~_<_cD4y)k?8 zWWdX0?W#&V=9`zj;(uwkkaa?STS%Xf>xJATx&48N$Hv?`4 z+zhxGa5La$z|DZ0f&XU)YVY>awHmAUeB1C>4Rz%vf~iUXf?;4=>VHw!P!mj)t0i={hm zy33}e(mgNTHKO=wssAlbW|ZfJ922-jnw`6aLaqG}b`W%G*iV@;r`fTo%Ewh=(X5HB fdFw3(58qst?4n3uY$zy;TZsd)iI(kqo|JzBQRRM& literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..476702fb100bc77d189496ee6001de6696741ee6 GIT binary patch literal 8984 zcmeHNYit|E79OXi$xUh3DGl@rDlLy72e^5ZS5$=XlBC$8P6KsYM!}t5RMrP>76lFO-7$&g?jG zHtL0+NbE>6=X~d!`DW(K*xu~qU+2#LRL_`i1!JrM)sOn-MT|WRN#&2IfjY)iHQF1C za?{d=I@A_{LBXi55N*t60FLbA?e=^V$8WY(h%y+1m!J+s;XPVeRucv zgsY!1nY*12B&h;#5M*?gsaDHXj7@Y9ke&56KyDx`iYzlW4ZR521xb6!{Z}$ZM==0` zdC1kM{iwBtVyRwSYjo+Sz_ZR}BX~I~jWhE7t>tL+mQ#=X<8)VH+tb=6*kn{6DqGvg zR!NOq=gV7lL)~>SBXtXf#*U#97tMvS%TRr+efzj&>zVegsbNzy$J@7pR)m|5kw+_Qfk+ z2D}V-8TdV9U`MF%sj^GisYJJjCcoMpD!d&o{CD!4FW)>_se7O$bl~UH1^h-Qdl&1cPpCiz0a%dtB z&4*+}&{;^(yoy-%3jOwXzE_&%wpF3RZ0RV46%LgZ$$yuh+HHZd)D$U}{sM>8+B=t& zS|c-M$=_QnO~I6u0%Ynfl6``A>wxW}q_qBYSz0#)=fFuAzJ#AZpyey4*U(H-+VwH4 z`a#708=iL{-$wXbNonw1qMI&Vg;U!PLu5vHFGq^ z8(72l=rr&nCir<$YW^GplH0#ADyH!<;$m2DU`G+eJFu((vV?K|hxN=Sr6aHVl2YtQ z=tbwS&)*R$tO(6^%LF0mXPzW~w1DJ~eW5}(r9IvO#x>vKi3AS%4HoG}x)f>inzMDt z))zDst!w>U61HPxJAnZyD+3rlFbiMo2$XN241P^PCCq5xPZoF)h@H7)PY?s-#7sto z+%05G$bKQ|;Sjsh6dw!CPEHU&NlH!6KMD!cU%A2ABc9!lXj{%vI)9*Cok!o7rzv*w z@b3GRLFLbiI&>&JS*g!Ah9}Saq^anSl?t#=diZ9pov)Xsg3!)fi-tZ=v-PX%>ewtE ztvl+64pmx?VJVfC6TCgi+yC(P6mL)S_7mQIhL*9d2R1$lMcH0yA$f4L9mX@CRVtOy zZKRzfZHTn@NZUi&o21=O+AE|TAngUxE+y?b(x9+2)6ht9l?j0-{{A;AXYQy<$38Fc z@ddl%KmKY&iEdTy+#K0Hs^#>==2Ui{mQE%5@|kcpm$xmpEvw5ZQUg}2Z0dXSDN|3# zfqk0U+1kcjy))WademZ3!;mvtc3d{{HhRjMIg-!lS=(CHfiw3R_Px56-l@lJ!(_xP zr!0BDX=F#*@Fd9C6=HF<5+4u#H}g%9RBsf2Uy@E=J{RMudA;-G?CU239x9twujKo~ zk!W5L{bq!;g?vcJM}>Ss$ftySM#x$&Pxmt5Wx&gTmjN#WUIx4jcp30A;AOzefR_O; z16~Gx8yRT2($C*GH3xCYmEf1^>M-twaS49U-iCv0ufI8{;<7!JjT;u+rfxG>H(U}_ zGx}JsX-2G(a6-u@qPX3MLtY2RShuNbw%%jphtqmQ!@c#Gdh3lHZt zW1TD5Tr*yVOw)Ulx#l^3^-u3jB)kQw{{I18dlQz$`wY5=1f+ju;AJyf;GYEKccrzM z?&pD>H!RTU6;`ePaR<||ygZ8*{slba!Y{h;Yc71$g-Uo)>aV-~wrOpB4&r&O^jS(4*r#8RKrV>rmB>t0c6XjiY(tBL)vY ZT#oFcNMIZ&D2qpl196C!oqL{?p8@m{4#WTe literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..9d323c076b9c1efade05313e16afc21c5ef9d427 GIT binary patch literal 8984 zcmeHNZEO@p7@jL1t)T1`Db%3JYEp9*(ht-?P~xp^^)7OyN`bpbU9Y{p_QKumvAYMY z_|c?jYz(F`F@D6DCML!hNNO~Z7{wYe#b}hEBuIz>19(+2c%TwQ&gY$->s_}&{ppXL zWafRIcV?cQd3W}9JJYW&oxM`Ym}eGatP0tSd~gn9Ps38W0ohle9UTJ)!r}k;djAo#nQ!gg;9u*qI5tdS#F}@x>ZI&kN)H@{f{6c~FwE32lIa1Xg z(=rn2Ug3A@c_q7IRMZ!_pwq_+r>bVt+Y{-Snn=fW&ZpnsF`?HX1c*#dHf}gH1$ad;V3hwZuh|WyVH<$6;5&(!J#MH#0^{(FcKZu=E&q zAeSdN(VgmVhz-En{{PTkEAKF)z2_NB z5#!F92B}l-S+=a9SzLtLuPPglC+5M===ApXlpF3XAJ?tuL-D#O+xNd-aEANhCN~0Z z1l$O?5ttc)4T0g)$|hx_(z!k`^m9jG`19cK??ab7*_xq3#p88>U2A;!EDP+qb0y4A z3&4-z_Z$2?O!Ii|0$xUe=kCf~gbfZ)5IoP}0fLtRcaB?L!a0tmosYpnroxuJ2?jEI z$caWD!9wO^dJXQQZ-wT=xtF+aZeVyK_c}Ej-k(<_@BLmX*VX2w>d<(u7Zp-{+o8Br z9~#R`-nQ{vCtPvKcLJ`qaq^GwZZdq|#-&Ba^U}f%Xdn3+jvrCKVHBX|L%UbkSX|nK znnfp(VI$wfrOijE)uG&?Wc}5nOK5*}N1!4Y8x-IEp4e zfoy=H?}?GT6uA^{1s#nmk}r>U^J)5A36kArN9js*7FxBJ{Zf#F$!i58hCEWPT<^3Mxc{^=JO zZt=yXB|Dnnp!Y9}OVvC4ARR=Yd);A7J1$+nBaC7ACQ;N(;wl(q9cZ!(#fY8g`IxAjUs1s(@M+yt*|on=cN9}9)E(pHabAAM%M-l(o|mV1 zc?Kn8Yj@rBB7m~KP)By8uMy74GlfE-ZyllIgt`bFCbWgnK|+ra+DB*?q4x+aCiFHT z05&-aM8J^-1s{C%6-tx$6s_$)C-}ET`*E>8q;#%T?p+;P-xtkjvDJz6wrDaDYtN>F z=}gu#*}AkQCkXn?dfCvnW)p@MlYQHwMss}wbM#I{`;&bp>(q5Q6-^Jwde%ZuIcoG~ zQ(D?GXLR7o?Yi}l7ENx{dMw>wq%0>)dAp&fdmHd2$k>fya^)6J0snZv1BUYL;_pks z^kZ@@zM2==U(Ozx9QdfLoe=q3 z7K-BBx?f#+dlS|Bc>VpF6(ZJDWbMRO6 ziu*bEt^eUrh(qjXfvcSI!G1rh&$K*!$1 zWwD<@=a8WEuM9k`MhSjJP<~cg&gpy}+}>dUrY)@U`sZ!Vw`%~|TMjtx;V^Nh~SBXV47MAAKml$gJ;<9Z!wFJk40w^sdPQ)UbvF>?L{stjr1*!l5 literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_LeftToe.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..a6bc56a9e500624f3980ee91bae8a392c1781fce GIT binary patch literal 8984 zcmeHNZ){sv6~E3(lcl52DHMuelG`nl!@9&NOjMmF;x#U|NJ5}VYO{nlCw@-M{IAb; zn@~UnXNWKg`80L#B@!U2eTqaSrXg)%?V7TcbySRHtduZmozoUby$!Z5Yxw={yLOzH z75IR}7hmb#bAIQX`@8p^ci;2NyE%L9uh(mZaNHz>s6%z4p8kLk4?(hg4AosDgs%7a zf<4l-x?zXPqA(}~>w4wW^{g?PT?w@Fd2Y6R#dVe>R;p!KCk|VR)#`dCn;XoOO?JL* z8!eYdMG!m6(O9eETbUpE^bsSTHcZ6X`Fa%J1B!zfX;~?2<~Ni|W>yw#=bN$3k9gI3 zSD2-y>qD8W8A}c;yPa=Q@f}zG5-!oKnk$UDo=YB#B_nz)8BIyN`uhG%NMx587}A2*QPccsc+x}J>dnX$xRDz2xq<_-C{S7iY&`eReSeTeORYHP|NbUhyack^>9zBU<+`^m3rGKjs+&XR$ zEJAKT?MAIEjAgy0(rDA?!L!L`Bm6ij$Jsjl!~V7}?<#)FXsV0+d}MA1Y%Wv>s@PaB z)@$`rm&;qfz2=iJBX#A%v4g1O;#`FID5^uW>>bNwjYLaFY|spwV=a5kaKbp4G9S#e z>`o1(!p+f=p{36-GcnwhEndqdS;))0t?){-XfxB{>_|&IHkcS~j*LNC_WvT@sO+#N z-tiqQ5%bPvOtP~M-Fatos~UvyJ1genh&b>VT^(;Nx%s}zdEJ3N%-7p4>-sk-EO1?_ zrJ8|i2L3l0==D#$=H2J*^Y-lZ=dX18C!PyT{4;;8Cbv1iT=Vrt|B-wDg!|Qh$B(@K;d=^?Lk1>JQT>HfPf~pX>iA67N&T4AwefmL(EQS(IcL!% zXngokT_Fuc@<9Kp)Ip@|U}}(H0FLn)ZM=X-52)f6)guwYAJi0*hy%5!!I%kTS(>GUm z0=B5;o~M1v>g0hZhZy&Tq854?j)9{9y)$SoMYVl*HDUyKt`xQY%V0`O13b$Sjv?X? z0KW(5eF1K1oh$Uoah-QBN!foj+OD~OgV=9O7u&VaSqvwmdyfG+@4CP!uNHbM>eKeU z0xkFkV*d=@3y|}4y@tU4^FXhnUZUl4Q43xs@7tujR`hAXcc|Talx*!<@I6G%F~B!N zJJyP!@^V6<1qS%I$d?%L5_*STWE9pAdXs355gh(KSgxTmg}u{^M&_KolDWvpx1wIn zy-2M1ov8F;%knNuV=W1rqLTd=)>Vz~{O8R}kNSnw`#Y zJZW2^;~g~4J`au0(tVcrfD^O42rcwFWB!5>on3B@%X|dcF;qZ^fveVkhUhuQUBbu< z81Y5aH=q~k!Bm15{Ik2=ygeaZO7i!ZSiS>5%J(z>ERrd6-EhbQBY!BdhKBpw0QeTg z9^Fa?m3%Xyfm`ySxnS&>lK3wWV5Z{B>vxzqK6Y zz+0C}f!E7{H^a8@cfiS2h0nLy+6^@|VivE`p4x#E%Z&x3Xu0u{Y_G`nZ?avKZBe#w z%Jv^(8Kg_bxY5PL8y|xNvcKd3odu)Xq~IpmvVhgVat_`x><;sU4wqoZ3gI zJw^?RSeSxF#a0e0tmNWnfAlEJ%StYk@Kc@Mo(}JQyE^xdgwsZ3S1frT9FIl1a*04P zoy%rK&%u-{M%A5Za+${dT+B2gF86`3*}5Igj;3Z|qez5D<0I&pO1To@0Pmm>!6uR4mT6-sDMoBC)5HfCEtVjck-tp zS#MK+15)RWatq#+KWx1%JLd5~MR7E2SSPQMMDNo)rZ}dQj3}8@a#YE0EBSpTA5rps zD66Jw2C5mTW}upZY6hwqsAiy=focY-8K`EUnt^Htsu}n{XP{xLQ+_Dh+=fH0h&)u+ z2XQWpL+~%|ubUM6otxWq9Ja@jL#YhhrjZq}ZuwA~o-jszrrDVp4n(}kNDof;;c#t& zV|2G^gtJC_DmNH6I>R`(9@RhdDG&W_>HnOO?IiM)P#4k866!Vy+awHRcBc~QxG_qT zUzsdsSnx%I^T3gB&7NFxD2wwwu{|1#8>LS-;dJbPb?zrxZxYj+@L^<<-%zGE&C6q5 zenZjlMWpoq2R!yBcB=0gJck6tr*f<8vhej1=GR8}%b4f$!16mKWB5s{(*FYsb68o9 zp@qMUr)~Ig8~(KoKX1c-v~YR8BqrcT{5n)Fn&;0v=jB-=O4|p~{HHjVV4ll4Cb&VG z?Yo7c)_&-;5wsiFPpL7x*|w=F$Mr_S=`6P9jkg#)a&uX-i;=+CP*7I3k_2iKt=ac7 GDgOf2CC&2y literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..183317a34b0842647795da95f2802f22f58d2538 GIT binary patch literal 8984 zcmeHNZ){sv6~9iqCQJW3$7|LgsPu*wz)uXOJ@zjMz0-E+^oj@@_rn>(Lw)H3GS!5FJUccQ;{4`a_jQu`dbyM{4U z^>+vS+_ZV136)J?P%!GdgilpXZNe-E+WC_ATD}){T9U9(EyGThu@q}m)s&eYN|j7@ zzM6fOt6fA8c9f#ASHV}#kNDJ4Etb>_#M$|d2)=WIgD}ccZd>vj))T4nhV6WFg71W2 zAiQb6WoE9a>Tt?5qKOeBM+65muv#h|clm=le6snpw<` zu$`mGO_I*ZQ50BJJ&Iqc@QCxk4;Gnin9g0XlUr32F*P+AAJSuL(loZ^qqE8z_-KBy z(eo_%psVVLUVaK&^Q#m26z34O^NFL$mr&JM=-=h%6nvdL8r!w;;j_l-g^vW)&5N$8 zJ-)v0bbEY$vZBd+EZ0lmCXpPSW$MNDMaDAi1Y~Rfb&ySjMU!R5=Ah3*9)+Z5$^8#7 zMn}gDf*j;-^b_cnjbf?qsWjU3KY_<(vl09}I<0g3t9PE)ZyfQw`SqXPcy-}#uN;O= zMt7jIef4aYRL^z3y);S8B9&0G+rf7sd{tJ6K!)WXjayZ9UN;BVmm1{)}#px3=8+SY=*c!W^pCZP|MM zEdnzlJ+7 zy%!&O9b$bS@V6~I4AC*4_2Wl^_qlIm1CYL%B~tsjx=8AGp-#`6PSU+xm!@CC-y6`3 zTQo0NG<)InKKK2)tdma$5A=GjgGkuH)WE?%K`}iqO+RZ<9kQqfEUI_Ni1n^Q`ev-z zOI%-i8-l$>UwIu@vvb^5>z!H7z6#YlvyfLLr{_T@iJk|W@=`&sh^?7*Afpa3dm}YW{#nwP-7ODR?C>x#FPMzZ#K(f5eo* z*}Qb-11KlwvwJAeBe@5`RKVoWyIu=$rX?US3vyqB{Kg5(7lMEaS`Fm zu-~QyvPkzX`X>>rvtEN-Zes2dBtio362~1V=Sq2OisG;Dk6 zx+Bs}W${SCk7>1=wAxKt<>2cyRgq4ugM6My5g$DGre($UvESdq@*$`f(BDDEt^pYq z+LCL`3GEi_d2Vk4`aRHx-sLVgeZC-NA?XX1F#i&Vy%gB^+KhWO}8-39MH1u}@ zoUp)s{Q+zv7m*rsSi|{4C!Xg9wBT41DjWPIGIU=d{5yib!Vq{L!GAz4uaRX9zB8Yq zDvkGIQ<(f$c-Cn`LWXt={WBc3^XIVSC@l$>^DM3M5d|G@q6OA5oI?Gn2JY+dt>Rgt zWFW(-pd~D}EHNd$%Tsg2ga-8`OCxo*tUg;-GbOcaf>iI+;9{YEOq(g@AJ^q5sqR#ZY-ktK zQ9}#M?y->3aR|fVmR4q?h=(R(qnN1cay*onl=ZZU>2k;zNyoK>nc6ae)A#A-Q(7oC zpbeY4!H8atrsQ!$PmHt@JX`_kH}V%m0V*Xu4ElHS5lHGg#J>T#4!j4yl=oY|mK_=V zZAWu7?A}Rq+qNGIj*O5VA^U_32stEVOvo`Iw7kg`8^XQyg!;9ww3FY> z{f}#AFQN7#>LYZth)x&;J2~{Fy7YK5rcIE^D@+zMEO?_qdEm;o=6E_WY~p^89g0L_ zTJhCQC>b5I?)_ND4wiG_ayLhJk~vp~U+dC46bWxcivRzBuDuE8_Ci6Udq_aM>bBZ3 z3vU!Ly|skDgz0`Bn7`(v7<%PZ>3`nB{5=}^e{RE(FV_bpK5EymZ%y)=69aPjND%I+topV4E`APYZ=w=b_g|;L&iN^hvwfcBm@XRl=d9 giKDsw5rc;xE=%@NBrpyXl=4yHKpdhi=bjhkUoM`Hx&QzG literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..7c285710f763f75ad23b2a4581d61e2b4a5d0390 GIT binary patch literal 8984 zcmeHNUrZcD7@vb$kk;2cXy_Lt9CzmmnkLpF8yp6G=kW}tM_2n?8s?9a6&D^xGp#j-N zU{Els%Y{!>Ev?U*@95;4Sz-J3U@j_zm)WY_E@MZsp{klzvcsG+Ir(<1vRwhugRp0= zH`Xfn=HnwiwMUEfY6ki_`7FWrwBR5tLbWZ6;_K8C=KO-4d{-Rt5nixfF;7OQYNu%# z(L}c}IQhJi?Km#{87$z!SYcGvWa4l%5mBRwE}i3r&v#VtH3|a43)Y(<7LxW!1rn^P z)k?FH<;?7lB_!>;Or=q(nuw`pf4oDFslAr5Bp>ZnUcgJ^Glib#oDaIHcI)#`VR3x< zBCgCB!cM;1Gvu5v7XGLByn?S%j3J!Ohj)z?2pp~5?YN|t-cT9$nir`~1 zN32+o(O#ySDfcsWBtSs6*PjnrOjs0IX6!ihF~|x?dY0V3o-x{rJ`l`6(reg=nq8yy|Z~4ZY z2Vu@Eox0H6t*FFBu`sp{)x-Qv{idbG{dLg}BW(2hYmIPRJFFWAOn;T$sfWwDG6w%% z%`l^QDgBD=;w;2Vd7J0uW>#tRhOHidEZPz8D~t3)+V=m@U&|h_sK4hqED>=hO@r8} zc5c{ERxT!?{uNpAcp@JB8C}@lUUJ!v?6@Al7|Pdm*?#_|0y8`px40Q_GvH>x&48PM zCxfZ0N{g~rX>JM*UJC?MAB9qX4c_o1iw4s<&z1y-w^iWtDLDMdPKaMt0soATTi{&a z^0BG4gr0wFsu(hqnjm!zS5K3AH`Jjq%S(Db*QKEs@DUH1)i&)Wo8}WTqSH4dQ{(T$ z%1%OOXKT10O0^wTeh9ByI%#_z!3WBL;=iWU*HjU%ixG*^>7*BKge3dR}K%>aduh9+wHw^gvCciZP8kk;=3 zPQgfvpFnSnY6Dg4Y1FEG8d~cO4xSF|Q}!!QDQf$8XfU0ZEC>zW^hiVct#lgLBOPtz z+RZ#^Xdkp`1|Cmmvdv=rdHEHdnT_EiMX_KTqL)uBwhDm#qv^z8xEXLW;AY_ek%7YXUbYnsK`ihrAk&zA8ftTUxcA?1*XgVcc8y zsarM&$nWF+otjloD3C!7geo$q(I8mKVaTk~jWTMl; z{T|!c6^&_`qnU7T^pJh;$I5fr%sL!~jMBNu%(^jt-A+d#5>7!f|9?Q&-h>0a}NBKjpyQ} zi3rfWGo2dJT{hi6(>*WUHKJ7Bf#%=($&B(mmtz7;q}jP!sH?pn>Kz2t8upXk?=(9$ nRrb6}B;0FZYc9RT;NhFgmJQSs7#j-8{8r*XY@$W`o+ssRYp=qo literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..68648289cf22c64ddd91077f861bca0eb83b845c GIT binary patch literal 8984 zcmeHNT}&KR6ut`xNNZ=Qq8M#5HNk+buvAS+O;eUXcT=EMus|%13p)#&?(Wc;SqjD` zB+w?rPXN&=YIFjnLFKKPQSeV+s!=2T-l7Vd~`SZ%q@)VgQRvhdTAD8iqcx&-pWmj2bz%C z1O^48o-2HcVyXk?Lf{gn8EKQ9uV$6y@`wn+u5>i6 zRq!qBkNA{cHPWZ*h_myVg6|o@K{&NlZd$UxZY^dkT(F(*f^C0<7hSJdCnXf6+c5QT ztVbB^d~V5d92EW(7I5)iVN{fOY)?2ARKl^4#_{6&J0bYC3j)H6t~W(2B%PBgBv?`E zysh30XX<<`BkA0wI?Ym)SVS=fqg`4==`;0Z`RJ_j240$$4t!Id$n2LTk696Bq4;vAfV+tfFPmFx6;9W4KEX=z|pvdLXLq(ezzLMXlDY14B3rC5aGt1|ZE3cB#m=;C}OBm)x~(3|oumrNXA!#30Pj>SqX^hNFHq zp>mtMMF!kE6Ou}QotHX(M1VR8OXq1=uEKI9B$dS{04~oFId|)zh;GB1liunP@Poz zIjufBUng~(K=3)5HH+v=nVb3pd-)Sy+A_A$F-1{FX~oUfik~6?S!mCOqcGP$-N0X8 zK{JKuufslSaCr&G|5lkJEXX&Rda{#isG9?aAw$N|wxEQ^}I!eEfosr}%i9k7xM! z4IjV5$XMgpntf2bEy)s+TV8#I+F2pzwk-Ci|F#@z^0qd5Yc@5t^alFW;HGe_KM)BAo8wV` ztS@dFY+Fo~!=#oPWwNgBj)!$MD3|sJ^vbewW}6)i3`BYj)~ac8G!PqF(ZH4xdMcAJ{ch*=IB@*Z7_^_25>DGbJ80{*wGK1oc|+_a*7{ zFs;N}^BvZkv&+MSnX7O`4!<98r`vHcZ&Jt+ArA=ovXHL}d05Ezg-jDUijx5+15O5< z3^*BZGT>yu$$*mqCj(9foD4V_a5C_}$w1*+Hx=jbMg^c%HWv@fC#kjW=Z7Beg;jnO_(^37_9 z$GT0__t^SSIHIOL%>??w{Z`$NRc5oHVtfplpzkI_#S^^TPM<;~`U*mYH#91H6ZVMj z8B~V^r2opm+a`>_HwEOi(o9VCd0_hF!W(_E%Jd(wu=NE7`dcf50ITb#@&&*>Z!SSHQ((?X%vd1$f`)Tua6+MwNR qJ5-tLdV_&J6GwCTBL)vYT$XI6NMIZ&C<{l4196C!oO@)0vA+TQYq!|| literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..d5b86d4c1713bfebb48aef25d03889b1bace4b45 GIT binary patch literal 9000 zcmeHNQEU`N7@jN8o~8BUfs%hjwjW23nObDV9LWmCrsx_9x2auSU7$1?eF&aanA%t-L-|TGfx=jr}`LdJD z{QvjQKi~fQ&ulZDxy!eHzf;3lXc=QHOf^dN(hA0olSFxtYGV~+oF}{XC535Wg8`LA zVxnNE)v}LsM;~^|j($GnUbk;&$d$m$%kBWjDk0~#Q%u?OCO==Q-tF2ZJ%A;(u!JRF zIX>|5Aw5^nE%Na5eIWT>mVCgJmU3AXU)nV6@(cU;|0fBlF!Q<7HnnSFrW8;NEXfq&GIZPjxQ|Z^7w%Le6=1K zNzQZHzr`1od~rDjxRQ^)HMX*X4|#(x=iQ0kXSzBQ-Z!R5C&|YoZj@wL%cx#?l(Azi z0E+t^he@slmPJt*n z6WYd+)|VtaMV&eBI2Q{67X z0$${;%qz^S-709#P%M{C<%gRyBP3n>UHI#j0~Yy*-lSKgxQn(0cGUEyP0g+HA@Faj zh$oZ@(Vx+U{_ZO`@2re#BaK16ZYu8lH%ZKBUV;*2Ajm+Ffgl4x27(Oyw+uWVAOBT- zLEWn+`{U!^C&vFCy%j3187)=4))1d~s*biI@rkYLN&Hq%_yTQb2>(Q)b!s*Y^!Agp zbtDtxGf-=U`YqIYQpcv8DD>MD)Yw#H>@AW+a~%UGwuXq7XugJ#oPHolG#9Z&!h=}r z)!9#ltvWtFGkccQ`1tebI#`d~`~A8fqf zvpY;q9nlxn0rjBD4@@UUOEtyX#OUo%WbD;asYEywIX)n?+clA~LDJ@Ws5wzC$k$BG z%Bm_hLtF8dngi3NhLgl!YB(j@)1v)cv}Z(nRdqZK{X<$o&+Nz=N3>iv(_73Zj6%_|*|UbOWT7_NO^T%-E@mw~qck4T ztk$Mx<{O>YhI2zUOPZ#V*NhRxEIKq&(X7E@UN;|4Ajm+Ffgl4x27(L(83-~EWFW{ukbxir z|8oXbJrdoI5Z1KQC09mVs`C`x3)3a|bBDtx*x~4!cAhqEM(W!8+j(9e?y{^NdoYnv zjZBiR^r=I+k2;1sEM0T-PP3TG={*`JwIJU=A> literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexmaci64 b/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..3739a8aaf7adc569d7e729b0f83a3aceb31a268d GIT binary patch literal 8984 zcmeHNUrZcD7@q?Kq<{9PqBK>rH9^5PaHx%>rYTpT-lafmK}uukdT-78=; zHh~(6!C>@79}G|0^g)U7p)s|dr72ijZR<^Y_ipa>o(WM;~^Q zneX@ezM0?7eme`x4ByM4w%+#TR>&iI5O>Y> z#=Q!?^!!Lq=~crqRY$*Uy&<9Zg3ur?rfPW>%&$v}8tEI()|(c3vd|+w?|y~fHj2_^ zn0hGMBOKX!ZpjLaiMY)Loj+GN6(t_+3q^xUDB7)YKL7cS3%zYZfcU)owS|SHby9-_ zD@wh$)teEt*T*83)}38xl%hn#iZKxB)WS;4)ECvGwaN>0X}n$NdCtXv6{SZ@KZS+! zD-`px#}LofyW1A$df~u7&Cf0LYQ-4hnROy79i&(x?#4*3U z{Ixlc!EN63 zH*`{`+O=j)Wwp2n^{>g8k1OcHkJ0)4ty`|#lsT_QFoyDVQ?{Oeh2RX&#VJk(oD4V_ za5CUz;NLRvoNxHDcZavl+uGtAyzcQ0f8`(kWAK(MUNV@pd;RHTA*_FRl3*@}#|ahz4kb)C;hXr?Ka`M$UW0|q3=JGvw+yn7 z`HY}OXXK)q@6#ZgPMS+;|x64;7N8%6_N27sr@pA&cky7b$UkH^#dHd z+p*+IORMA~Zg-uXsh8Z#4iWK6JI{7ENIMch?W8$Pu3x*Q9XBBH3mob=NZA2vWcu2_ z;VQq=AY5wab<|6!6W#Sv`L`JKCryzi^A~1zlO*A3pM~Qh?2o9c;4FT~Ke?HI$dXW! zx}8Mm6oD@RP7^o>a2BhxJ|T1;p+kfK z*wry00`trC#&_SLa$%$J@3(D}x3$r`X?;^mZy=@y*N39}0^v}wIUeyxV{y}9TcfHR zB3N!z$hx{W9@5pIT)r=$S65WBtkID`f4J9Rt(qoB0?`3ki<=lJ2lSqJM2(upf&pB) zSu?k*fpDAJWokMjWjSQXeYzIysl=BcV-JYQW?H;G{BPzbVX3x=zb^^X%d-?;&G%Yg z&Mpsi#z$rGl02S&&O~!jj5{Oj+rqvl>_K5a7WRm+Cxp$^a3klXwp@6jEvn4;8y2^AV1N&1@$oEvIE#Og#lOzt7cD+FU%C(x zI(Me6K|0H(^JhBerL#tq+GS||t)I*&&vQ8@vPhbF=YzW-4tjol+sun7j_ml-QC&F%(kFS zYyveQ27`$&`p`7-!KM#N)Q3iErKKroZKZuMZDUN0X|qjRgCuILP1o<-xdY1*tdBm- zP0l^%JLleS?wq*;!##X|`{vyO#yq)*Sit#fg;Q0N@ja1vNR7n9I>+;$@2KG0EC>kCyI+S`Xxb+= zNU*9lC~Znc(Agi0Xxev<&@5GrM^&>w)}=?)gk>zsM|+hw@X~yz(es>*!B*8CeeNkN zTwkGBmotZOHsAdYIp>Q8|80FoK+DF)hJf>q$#7B= zd`#wul?pQ2%Op;Fg0X!*0XaSqpesDf@$a_kj;?H zMq^3DE1DUawJ(Eb#VndL9)U^M`Eu!>pG6+8E&OoH8wcJz9eVc}_+*F&!j>1Ye5r`r z{PyzKsMqza(}Llb1?a`PmaMKA})EHBx&@T-4tOd1@52F(mj9A3K& zw4nKloEUV5^w9X%@UmmDS@rqUN6>+xeE-ly>SJ!6nvuNqUUVBvXQblh3#op%VKq(? zoI;@SLQ3O4Z>tQLoD56lS7)T{KVyJ422bY{JlEk#g{6wv=#12Hg+dqLxd@q_k#_t9 z2OoAKwbJG)`Go5o=Vlrt@3O-L6lwdpaHF(s3|0qmPLu1mu(a(aD1L=Q8wDuaZ_mWF zF?H)ySdq$SaGl?2+B8P}MQfs~{|&i><>2buZ$K{FLqU21-U(tq4r3gW3s9UNnApI- zWr-<;31<=cj)r{$;|v@>K%axO7Si~3uYcgA?-gZ-@~WbCjt2(P1<9hoz#We?xc6>4 z4eXKjcXI1afi&0)>skxC5z%PBOcX51$zc=t^!62Wj;G6x;O5e0U-14Y@9EK{%f@(r zg7+tRe+oTgO~a2RVJNNXGSVZxRd8Ornog&CH<9%tS)F8^Cu=8JlVrV4)-ke%$@-kE zhsZii77TXnAS^QGx9N2syie`oI^o~v=w_v@Nm*ap+}ayVXrbCje0MM!3AH3+fp{Wm znQT*BlOtr7n-#L5?Mg-rEhLxk4jR=Jl`Ly^EZ7(AHCdak%dud*U)GZrX39aMCmGY? zmbqX8XKvB0Em|W)Xvgzen zim&De?JsAKj|StTvUo`z&p$_^yCmjK3H_eX2ZTN(^ru1}75bFWnOv^zX28vWn*lci zZU)>8xEXLW;AX(hfSUm~18xS~4E#SbP`t`ZWi?h(hay*q7uD4+)P+$5e|cBo2;1c? zsZ&vGkHoum6JbNM7`&wq)TuG8uhB4?&7MF=iHF)y-A6!Pfk0oqp#?3iK~Hu?wdNq| z)_v;p&-y4{&f_m>Rx=@=gIWmHIB2s$u#UrkS+Bwc^{myML-Fl3C*O-4#bdAXgALL{7mIRAe@WpBcD;yi=u zkbv~B47}B%2fiyHua#zEs?P({kqd8hM3otT$Hw*v4D3T$cr*(i$->98@YyVU$;Pwm zr3(?Dx-*>%QZ1Y6&s687S|eKBGIaMECo{_PY>o*mlIHB)LPPES(40lkpkY7h{n_rU oO_h0FB@|3p*qV!PF+}+0vULlM1jdGfF}IaC5SwVhzUN8#2PTQMqyPW_ literal 0 HcmV?d00001 From c745c825f823e07308d00e33ee1e4c7db59a92b4 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 12 Oct 2021 04:32:26 -0700 Subject: [PATCH 20/49] refactor rollout interface for CtrlAffineSysFL and multiple reset simulation. --- ...t_controller_for_multiple_resets_complex.m | 156 ---------------- ...ut_controller_for_multiple_resets_simple.m | 132 ------------- demos/rabbit/run_clf_simulation_rabbit.m | 50 ++--- demos/run_cbf_clf_simulation_acc.m | 2 +- demos/run_clf_simulation_inverted_pendulum.m | 2 +- lib/@CtrlAffineSysFL/ctrlClfQpFL.m | 40 ++-- lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m | 101 +++++----- lib/sim/get_ctrl_ref_from_ctrl_previous.m | 15 ++ lib/sim/rollout_controller.m | 172 ++++++++++------- lib/sim/rollout_controller_eval_clf_FL.m | 175 ------------------ lib/sim/rollout_controller_eval_clf_FL_orig.m | 170 ----------------- .../rollout_controller_for_multiple_resets.m | 139 ++++++++++++++ 12 files changed, 365 insertions(+), 789 deletions(-) delete mode 100644 demos/rabbit/rollout_controller_for_multiple_resets_complex.m delete mode 100644 demos/rabbit/rollout_controller_for_multiple_resets_simple.m create mode 100644 lib/sim/get_ctrl_ref_from_ctrl_previous.m delete mode 100644 lib/sim/rollout_controller_eval_clf_FL.m delete mode 100644 lib/sim/rollout_controller_eval_clf_FL_orig.m create mode 100644 lib/sim/rollout_controller_for_multiple_resets.m diff --git a/demos/rabbit/rollout_controller_for_multiple_resets_complex.m b/demos/rabbit/rollout_controller_for_multiple_resets_complex.m deleted file mode 100644 index 25c33f8..0000000 --- a/demos/rabbit/rollout_controller_for_multiple_resets_complex.m +++ /dev/null @@ -1,156 +0,0 @@ -function [xs, us, ts, extras] = rollout_controller_for_multiple_resets_complex(... - x0, plant_sys, control_sys, controller, dt, ... - event_func, reset_map_function, nstep, ... - varargin) -% Rollout control for the hybrid system that uses reset_map functions -% Input -% Necessary -% event_func: function handle that detect the contact -% reset_map_function: reset the state after detection of the contact -% nstep: number of contacts -% Default -% varargin: with_slack, mu0, verbose, event_func -% -% Output: -% xs: trajectory -% us: control inputs -% mus: virtual control inputs -% ts: time stamps -% etas_zs: [y dy phase dphase] -% feas: socp feasibility -% slacks: slack - - settings = parse_function_args(varargin{:}); - - if ~isfield(settings, 'with_slack') - with_slack = 0; - else - with_slack = settings.with_slack; - end - - if ~isfield(settings, 'mu0') - mu0 = []; - else - mu0 = settings.mu0; - end - - if ~isfield(settings, 'verbose') - verbose = 0; - else - verbose = settings.verbose; - end - - %% Record Paper - x = x0; - t0 = 0; % TODO: t0 in varargin(?) - % initialize traces. - xs = []; - etas_zs = []; - ts = []; - Vs = []; - us = []; - mus = []; - Fsts = []; - ys = []; - dys = []; - dVs_error = []; - feas = []; - slacks = []; - step_logs = []; - %% Run Step Simulation - for k = 1:nstep - step_summary = strcat("[Step]", num2str(k)); disp(step_summary); - - % Initialize all robot status each step - is_walking = false; - - % change this to lyapunov controller - [xs_new, us_new, ts_new, extras_new] = rollout_controller_eval_clf_FL( ... - x0, t0, plant_sys, control_sys, controller, dt, ... - 'with_slack', with_slack, 'verbose', verbose, 'event_func', event_func); - % extras_new: y, dy, Fst, mu, - - Vs_new = extras_new.logs.Vs; - dVs_error_new = extras_new.logs.dVs_error; - etas_zs_new = extras_new.logs.etas_zs; - - ys_new = extras_new.ys; - dys_new = extras_new.dys; - Fsts_new = extras_new.Fsts; - mus_new = extras_new.mus; - feas_k = extras_new.feas; - slacks_k = extras_new.slacks; - - % TODO: Reset Map when event is detected, no uncertainty term related - x0 = reset_map_function(xs_new(end, :)); - t0 = ts_new(end); - - % If robot has fallen, display the robot's status and stop - % collecting datas - height_threshold = control_sys.params.fall_threshold; % threshold to judge if robot has fallen. - height_trajectory = xs_new(:, 2); - last_valid = find(height_trajectory transpose different - % use eval_FL -run_simple = true; -if run_simple - [xs, us, ts, extras] = rollout_controller_for_multiple_resets_simple(... - x0, plant_sys, control_sys, simple_controller, dt, ... - event_func, reset_map_function, nstep,... - 'with_slack', with_slack, 'verbose', verbose); - xs = xs'; - ts = ts'; -else - [xs, us, ts, extras] = rollout_controller_for_multiple_resets_complex(... - x0, plant_sys, control_sys, complex_controller, dt, ... - event_func, reset_map_function, nstep,... - 'with_slack', with_slack, 'verbose', verbose); -end +[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); +xs = xs'; +ts = ts'; %% Step 5. Plot the result -if ~run_simple - plot_rabbit_result(xs, ts, us, extras); -end +% plot_rabbit_result(xs, ts, us, extras); %% Step 6. Five Link Animation animation_scale = plant_sys.params.scale; diff --git a/demos/run_cbf_clf_simulation_acc.m b/demos/run_cbf_clf_simulation_acc.m index 363706a..67a7196 100644 --- a/demos/run_cbf_clf_simulation_acc.m +++ b/demos/run_cbf_clf_simulation_acc.m @@ -38,7 +38,7 @@ acc_sys = AccBuiltIn(params); [xs, us, ts, extraout] = rollout_controller( ... - x0, acc_sys, acc_sys, @acc_sys.ctrlCbfClfQp, sim_t); + x0, acc_sys, acc_sys, @acc_sys.ctrlCbfClfQp, sim_t, 'verbose_level', 1); Vs = extraout.Vs; Bs = extraout.Bs; slacks = extraout.slacks; diff --git a/demos/run_clf_simulation_inverted_pendulum.m b/demos/run_clf_simulation_inverted_pendulum.m index dd870dd..e3bef8c 100644 --- a/demos/run_clf_simulation_inverted_pendulum.m +++ b/demos/run_clf_simulation_inverted_pendulum.m @@ -26,7 +26,7 @@ ip_sys = InvertedPendulum(params); [xs, us, ts, extraout] = rollout_controller( ... - x0, ip_sys, ip_sys, @ip_sys.ctrlClfQp, sim_t); + x0, ip_sys, ip_sys, @ip_sys.ctrlClfQp, sim_t, 'verbose_level', 1); figure; title('Inverted Pendulum: CLF-QP States'); diff --git a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m index 175b295..f6ac107 100644 --- a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m +++ b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m @@ -1,5 +1,5 @@ %% Author: Jason Choi (jason.choi@berkeley.edu) -function [mu, extra_t] = ctrlClfQpFL(obj, s, mu_ref, with_slack, verbose) +function [mu, extraout] = ctrlClfQpFL(obj, x, varargin) %% Implementation of CLF-QP under feedback linearization structure. % Inputs: s: state % mu_ref: reference virtual control input @@ -13,22 +13,34 @@ % when qp is infeasible, u is determined from quadprog.) % TODO: AffinesystemFL => u_ref, mu_ref (varargin input!) - if nargin < 3 + 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; + verbose = 1; + 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 - [y, dy, L2fy, LgLfy, ~] = 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); @@ -119,7 +131,9 @@ [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. CBF constraint is conflicting with input constraints."); + end else feas = 1; end @@ -132,14 +146,18 @@ [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. CBF constraint is conflicting with input constraints."); + end else feas = 1; end slack = []; end + comp_time = toc(tstart); - extra_t.slack = slack; - extra_t.feas = feas; - extra_t.Vs = V; + 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 b2aa912..41c3df1 100644 --- a/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m +++ b/lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m @@ -1,69 +1,82 @@ -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. +% 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 -y = obj.y(s); +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 = obj.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; -%% Clip input into the physical constraint -eps = 1e-4; -if isfield(obj.params, 'u_max') - if size(obj.params.u_max, 1) == 1 % row vector - for i = 1:obj.udim - if u(i) > obj.params.u_max + eps - 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 % column vector - for i = 1:obj.udim - if u(i) > obj.params.u_max(i) + eps - 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 - eps - 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) - eps - 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.feedforward = feedforward; +extraout.u_raw = u_raw; \ No newline at end of file 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 index 46c8a30..e27862c 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -23,15 +23,11 @@ % 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 handle that ends the simulation, for more +% 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) -% ratio_u_diff: ratio(0.0~1.0) in the min-norm objective for minimizing the -% difference between u and u_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}) % u_ref: reference signal of u. It can be a constant vector or a function % handle that takes x as input. % DEBUG: @@ -53,13 +49,17 @@ % 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_FL_system = true; + is_control_sys_FL = true; else - is_FL_system = false; + is_control_sys_FL = false; end if ~isfield(settings, 't0') @@ -89,17 +89,44 @@ 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 isa(settings.mu_ref, 'function_handle') - error("Not supported in the current version."); - elseif length(settings.mu_ref) ~= control_sys.udim + 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 @@ -112,12 +139,6 @@ with_slack = settings.with_slack; end -if ~isfield(settings, 'ratio_u_diff') - ratio_u_diff = 0; -else - ratio_u_diff = settings.ratio_u_diff; -end - if ~isfield(settings, 'verbose_level') if isfield(settings, 'verbose') verbose_level = settings.verbose; @@ -128,10 +149,10 @@ verbose_level = settings.verbose_level; end -if ~isfield(settings, 'end_event') - end_event = []; +if ~isfield(settings, 'end_event_function') + end_event_function = []; else - end_event = settings.end_event; + end_event_function = settings.end_event_function; end if ~isfield(settings, 'ode_func') @@ -158,20 +179,17 @@ us = []; % traces for extras, they remain empty if not returned. feas = []; +comp_times = []; +slacks = []; Vs = []; Bs = []; -feas = []; -slacks = []; -comp_times = []; - -mus = []; +% traces for extras, specific to feedback linearize-based controller. ys = []; -dys = []; +mus = []; % Initialize state & time. x = x0; t = t0; - u_prev = zeros(control_sys.udim, 1); mu_prev = zeros(control_sys.udim, 1); @@ -180,29 +198,32 @@ % _t indicates variables for the current loop. while ~end_simulation %% Determine control input. - % TODO: can be simpler, but confusing. if t == t0 && ~isempty(u0) u = u0; + elseif t == t0 && ~isempty(mu0) + mu = mu0; + u = control_sys.ctrlFeedbackLinearize(x, mu); else - if isempty(u_ref) - u_ref_t = ratio_u_diff * u_prev; - else - u_ref_t = u_ref; - end - if is_FL_system - % TODO: going to support u_ref, mu_ref ver. - %[u, extra_t] = controller(x, u_ref_t, mu_ref_t, with_slack, verbose); - % mu_ref_t should be supported - if isempty(mu_ref) - mu_ref_t = ratio_u_diff * u_prev; + 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 - mu_ref_t = mu_ref; + u_ref_t = u_ref; end - [u, extra_t] = controller(x, mu_ref_t, with_slack, verbose); - else [u, extra_t] = controller(x, 'u_ref', u_ref_t, ... - 'with_slack', with_slack, 'verbose', (verbose_level>=2)); - end + '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); @@ -210,7 +231,7 @@ us = [us, u]; feas = [feas, extra_t.feas]; - comp_times = [comp_times, comp_times]; + comp_times = [comp_times, extra_t.comp_time]; if with_slack slacks = [slacks, extra_t.slack]; end @@ -220,19 +241,28 @@ 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, '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) - ode_opt = odeset('Events', end_event); + 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 = (ts_t(end) == t0 + T) || ~isempty(t_event); + end_with_event = ~isempty(t_event); else [ts_t, xs_t] = ode_func(@(t, x) plant_sys.dynamics(t, x, u), ... [t, t_end_t], x); end_simulation = ts_t(end) == t0 + T; + end_with_event = []; end t = ts_t(end); x = xs_t(end, :)'; @@ -241,27 +271,36 @@ %% Record traces. xs = [xs, x]; ts = [ts, t]; - if is_FL_system - ys = [ys, extra_t.y]; - dys = [dys, extra_t.dy]; - mus = [mus, extra_t.mu]; - end end % end of the main while loop %% Add control input for the final timestep. -if isempty(u_ref) - u_ref_t = ratio_u_diff * u_prev; -else - u_ref_t = u_ref; +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 -[u, extra_t] = controller(x, 'u_ref', u_ref_t, ... - 'with_slack', with_slack, 'verbose', (verbose_level>=2)); if verbose_level >= 1 print_log(t, x, u, extra_t); end + us = [us, u]; feas = [feas, extra_t.feas]; -comp_times = [comp_times, comp_times]; +comp_times = [comp_times, extra_t.comp_time]; if with_slack slacks = [slacks, extra_t.slack]; end @@ -271,9 +310,10 @@ if isfield(extra_t, 'Bs') Bs = [Bs, extra_t.Bs]; end -if is_FL_system +if isfield(extra_t, 'y') ys = [ys, extra_t.y]; - dys = [dys, extra_t.dy]; +end +if isfield(extra_t, 'mu') mus = [mus, extra_t.mu]; end @@ -289,10 +329,14 @@ if ~isempty(Bs) extraout.Bs = Bs; end -if is_FL_system - extraout.mus = mus; +if ~isempty(ys) extraout.ys = ys; - 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. diff --git a/lib/sim/rollout_controller_eval_clf_FL.m b/lib/sim/rollout_controller_eval_clf_FL.m deleted file mode 100644 index 07925c1..0000000 --- a/lib/sim/rollout_controller_eval_clf_FL.m +++ /dev/null @@ -1,175 +0,0 @@ -% function [xs, us, mus, ts, Vs, dVs_error, etas_zs, feas, slacks, extras] = rollout_controller_eval_clf_FL( ... -% x0, t0, plant_sys, control_sys, controller, dt, sim_t, with_slack, mu0, verbose, event_func) -function [xs, us, ts, extras] = rollout_controller_eval_clf_FL( ... - x0, t0, plant_sys, control_sys, controller, dt, varargin) -%% Parse varargin into settings -% Support following symbols for varargin - % with_slack - % mu0 = reference for mu input - % verbose = 0(run quitely), 1(show QP info) - % event_func = run simulation until event happens - % sim_t = simulation time - -settings = parse_function_args(varargin{:}); - -if ~isfield(settings, 'mu0') - mu0 = []; -else - mu0 = settings.mu0; -end - -if ~isfield(settings, 'with_slack') - with_slack = 0; -else - with_slack = settings.with_slack; -end - -if ~isfield(settings, 'verbose') - verbose = 0; -else - verbose = settings.verbose; -end - -if ~isfield(settings, 'event_func') - event_func = []; -else - event_func = settings.event_func; -end - -if ~isfield(settings, 'sim_t') - % the dynamics that engage reset map should not be judged by sim_t - % set sim_t to enough time - sim_t = 100; -else - sim_t = settings.sim_t; -end - -%% Prepare record paper -% Total time step -total_k = ceil(sim_t / dt); - -% Initialize traces. -xs = zeros(1, plant_sys.xdim); % trace of the simulation. -ys = zeros(1, control_sys.ydim); % Wonsuhk Revised -ts = zeros(2, 1); -Vs = zeros(2, 1); -dys = zeros(1, control_sys.ydim); % Wonsuhk Revised -us = zeros(1, control_sys.udim); -mus = zeros(1, control_sys.udim); -dVs_error = zeros(1, 1); -feas = false(1, 1); % added -slacks = zeros(1, 1); % added -F_sts = zeros(1, 2); % added - -x = x0; xs(1, :) = x0; -t = t0; ts(1) = t; - -[y, dy, ~, ~, ~] = control_sys.eval_y(x); - -z = control_sys.z(x); -ys(1, :) = y; -dys(1, :) = dy; -Vs(1) = control_sys.clf_FL(y, dy); -zs(1, :) = z; -%% Run Simulation - -for k = 1:total_k-1 - % Evaluate CLf related values - z = control_sys.z(x); - V = control_sys.clf_FL(y, dy); - LfV = control_sys.lF_clf_FL(y, dy); - LgV = control_sys.lG_clf_FL(y, dy); - - %% Determine control input - if k ==1 && ~isempty(mu0) - % Apply predetermined control input for the initial step. - mu = mu0; %mu0 - u = control_sys.ctrlFeedbackLinearize(x, mu); - dV_hat = LfV + LgV * mu; % model based estimate of dV - feas_k = true; - slack = 0; - else - % Determin control input from the controller. - [mu, extra_t] = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); % mu - u = control_sys.ctrlFeedbackLinearize(x, mu); % u=u_star + LgLf\mu - dV_hat = LfV + LgV * mu; % model based estimate of dV - end - - slack = extra_t.slack; - V = extra_t.Vs; - feas_k = extra_t.feas; - - feas(k, :) = feas_k; - us(k, :) = u; - mus(k, :) = mu; - slacks(k, :) = slack; - - % Run simulation for one time step with determined control input. - if isempty(event_func) - [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! - else - event_options = odeset('Events', event_func); - [ts_k, xs_k, te, xe, ~] = ... - ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! - end - - t = ts_k(end); - ts(k+1) = t; - x_next = xs_k(end, :)'; - % x_next = clip_theta(xs_k(end, :))'; - xs(k+1, :) = x_next; - - %% Recording Section: Evaluate CLF related values after one time step. - % Record dVs_error(k) = dV - dV_hat - % Vs = V_next - [y_next, dy_next, ~, ~, ~] = control_sys.eval_y(x_next); - z_next = control_sys.z(x_next); - ys(k+1, :) = y_next; % Wonsuhk_revised - dys(k+1, :) = dy_next; - V_next = control_sys.clf_FL(y_next, dy_next); - LfV_next = control_sys.lF_clf_FL(y_next, dy_next); - LgV_next = control_sys.lG_clf_FL(y_next, dy_next); - % Estimating Vdot based on second order numerical differentiation. - dV_hat_next = LfV_next + LgV_next * mu; - dV_hat = (dV_hat + dV_hat_next)/2; - % Estimation of actual Vdot based on numerical differentiation - dV = (V_next - V) / dt; - dVs_error(k) = dV - dV_hat; - Vs(k+1) = V_next; - zs(k+1, :) = z_next; - - [F_st_u_next, F_st_nu_next] = plant_sys.get_force(x_next); - F_st_next = F_st_u_next*u + F_st_nu_next; - F_sts(k, :) = F_st_next'; - - if verbose - fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... - [dV, dV_hat, dV-dV_hat, LfV, x(3), x(4)]); - end - - if ~isempty(te), break; end - - x = x_next; - y = y_next; - dy = dy_next; - % phase = phase_next; - -end % end of main for loop - -% Cache the training data in aggregated form -etas = [ys,dys]; -etas_zs = [etas zs]; - -%% Archive extra datas -% training data required to train the model (refer to GP-CLF-SOCP ver) -extras.logs.Vs = Vs; -extras.logs.dVs_error = dVs_error; -extras.logs.etas_zs = etas_zs; - -% Tune this in order to retrieve the specific params you want to save -extras.feas = feas; -extras.slacks = slacks; -extras.mus = mus; -extras.ys = ys; -extras.dys = dys; -extras.Fsts = F_sts; \ No newline at end of file diff --git a/lib/sim/rollout_controller_eval_clf_FL_orig.m b/lib/sim/rollout_controller_eval_clf_FL_orig.m deleted file mode 100644 index 1801182..0000000 --- a/lib/sim/rollout_controller_eval_clf_FL_orig.m +++ /dev/null @@ -1,170 +0,0 @@ -function [xs, us, ts, extras] = rollout_controller_eval_clf_FL_orig( ... - x0, t0, plant_sys, control_sys, controller, dt, varargin) -%% Parse varargin into settings -% Support following symbols for varargin - % with_slack - % mu0 = reference for mu input - % verbose = 0(run quitely), 1(show QP info) - % event_options = run simulation until event happens - % sim_t = simulation time - -settings = parse_function_args(varargin{:}); -if ~isfield(settings, 'with_slack') - with_slack = 0; -else - with_slack = settings.with_slack; -end - -if ~isfield(settings, 'mu0') - mu0 = []; -else - mu0 = settings.mu0; -end - -if ~isfield(settings, 'verbose') - verbose = 0; -else - verbose = settings.verbose; -end - -if ~isfield(settings, 'event_func') - event_func = []; -else - event_func = settings.event_func; -end - -if ~isfield(settings, 'sim_t') - % the dynamics that engage reset map should not be judged by sim_t - % set sim_t to enough time - sim_t = 100; -else - sim_t = settings.sim_t; -end - -%% Prepare record paper -% Total time step -total_k = ceil(sim_t / dt); - -% Initialize traces. -xs = zeros(1, plant_sys.xdim); % trace of the simulation. -ys = zeros(1, control_sys.ydim); % Wonsuhk Revised -ts = zeros(2, 1); -Vs = zeros(2, 1); -dys = zeros(1, control_sys.ydim); % Wonsuhk Revised -us = zeros(1, control_sys.udim); -mus = zeros(1, control_sys.udim); -dVs_error = zeros(1, 1); -feas = false(1, 1); % added -slacks = zeros(1, 1); % added -F_sts = zeros(1, 2); % added - -x = x0; xs(1, :) = x0; -t = t0; ts(1) = t; - -[y, dy, ~, ~, ~] = control_sys.eval_y(x); - -z = control_sys.z(x); -ys(1, :) = y; -dys(1, :) = dy; -Vs(1) = control_sys.clf_FL(y, dy); -zs(1, :) = z; -%% Run Simulation - -for k = 1:total_k-1 - % Evaluate CLf related values - z = control_sys.z(x); - V = control_sys.clf_FL(y, dy); - LfV = control_sys.lF_clf_FL(y, dy); - LgV = control_sys.lG_clf_FL(y, dy); - - %% Determine control input - if k ==1 && ~isempty(mu0) - % Apply predetermined control input for the initial step. - mu = mu0; %mu0 - u = control_sys.ctrlFeedbackLinearize(x, mu); - dV_hat = LfV + LgV * mu; % model based estimate of dV - feas_k = true; - slack = 0; - else - % Determin control input from the controller. - [mu, extra_t] = controller(x, zeros(control_sys.udim, 1), with_slack, verbose); % mu - u = control_sys.ctrlFeedbackLinearize(x, mu); % u=u_star + LgLf\mu - dV_hat = LfV + LgV * mu; % model based estimate of dV - end - feas_k = extra_t.feas; - slack = extra_t.slack; - V = extra_t.Vs; - feas(k, :) = feas_k; - us(k, :) = u; - mus(k, :) = mu; - slacks(k, :) = slack; - - % Run simulation for one time step with determined control input. - if isempty(event_func) - [ts_k, xs_k] = ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x); %TODO => eventfunc!! - else - event_options = odeset('Events', event_func); - [ts_k, xs_k, te, xe, ~] = ... - ode45(@(t, s) plant_sys.dynamics(t, s, u), [t t+dt], x, event_options); %TODO => eventfunc!! - end - - t = ts_k(end); - ts(k+1) = t; - x_next = xs_k(end, :)'; - % x_next = clip_theta(xs_k(end, :))'; - xs(k+1, :) = x_next; - - %% Recording Section: Evaluate CLF related values after one time step. - % Record dVs_error(k) = dV - dV_hat - % Vs = V_next - [y_next, dy_next, ~, ~, phase_next] = control_sys.eval_y(x_next); - z_next = control_sys.z(x_next); - ys(k+1, :) = y_next; % Wonsuhk_revised - dys(k+1, :) = dy_next; - V_next = control_sys.clf_FL(y_next, dy_next); - LfV_next = control_sys.lF_clf_FL(y_next, dy_next); - LgV_next = control_sys.lG_clf_FL(y_next, dy_next); - % Estimating Vdot based on second order numerical differentiation. - dV_hat_next = LfV_next + LgV_next * mu; - dV_hat = (dV_hat + dV_hat_next)/2; - % Estimation of actual Vdot based on numerical differentiation - dV = (V_next - V) / dt; - dVs_error(k) = dV - dV_hat; - Vs(k+1) = V_next; - zs(k+1, :) = z_next; - - [F_st_u_next, F_st_nu_next] = plant_sys.get_force(x_next); - F_st_next = F_st_u_next*u + F_st_nu_next; - F_sts(k, :) = F_st_next'; - - if verbose - fprintf("dV: %.4f \t dV_hat: %.4f \t dV-dV_hat: %.4f \t LfV: %.4f \t theta: %.4f \t dtheta: %.4f\n", ... - [dV, dV_hat, dV-dV_hat, LfV, x(3), x(4)]); - end - - if ~isempty(te), break; end - - x = x_next; - y = y_next; - dy = dy_next; - % phase = phase_next; - -end % end of main for loop - -% Cache the training data in aggregated form -etas = [ys,dys]; -etas_zs = [etas zs]; - -%% Archive extra datas -% training data required to train the model (refer to GP-CLF-SOCP ver) -extras.logs.Vs = Vs; -extras.logs.dVs_error = dVs_error; -extras.logs.etas_zs = etas_zs; - -% Tune this in order to retrieve the specific params you want to save -extras.feas = feas; -extras.slacks = slacks; -extras.mus = mus; -extras.ys = ys; -extras.dys = dys; -extras.Fsts = F_sts; \ No newline at end of file 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..a53301c --- /dev/null +++ b/lib/sim/rollout_controller_for_multiple_resets.m @@ -0,0 +1,139 @@ +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) +% TODO: 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, '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 = []; +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_event = 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_event; + if verbose_level >= 1 + print_step_summary(k, step_summary); + end + % Save to log. + %% TODO: this can be changed based on the exit_function. + if end_with_event && exclude_pre_reset + index_last = length(ts_new)-1; + else + index_last = length(ts_new); + 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_event + % One step rollout terminated without hitting the reset. Terminate + % the simulation here. + break; + end + %% reset to next initial state. + x0 = reset_map_function(xs_new(:, end)'); + t0 = ts_new(end); +end +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}); + 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 + +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 From 20d7d3b5acb6e4d4f01da0673cc7d8506f971758 Mon Sep 17 00:00:00 2001 From: jason Date: Wed, 13 Oct 2021 15:55:53 -0700 Subject: [PATCH 21/49] fix bugs. --- dynsys/@InvertedPendulum/defineClf.m | 20 +++++++----- lib/@CtrlAffineSys/ctrlCbfClfQp.m | 8 +++-- lib/@CtrlAffineSys/ctrlCbfQp.m | 15 +++++++-- lib/@CtrlAffineSys/ctrlClfQp.m | 17 +++++++--- lib/sim/rollout_controller.m | 32 ++++++++++++++++--- .../rollout_controller_for_multiple_resets.m | 8 ++--- 6 files changed, 74 insertions(+), 26 deletions(-) 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/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index 15b9db9..ddc6a52 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -117,7 +117,9 @@ [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 @@ -132,7 +134,9 @@ [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 diff --git a/lib/@CtrlAffineSys/ctrlCbfQp.m b/lib/@CtrlAffineSys/ctrlCbfQp.m index 89b2c59..47fb3e0 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -98,8 +98,17 @@ [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; @@ -113,7 +122,9 @@ [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 diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index 3a8dc9a..78267c5 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -98,11 +98,16 @@ [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 @@ -116,7 +121,9 @@ [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 diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index e27862c..6136f0a 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -5,10 +5,18 @@ %% [xs, us, ts, extras] = rollout_controller( ... %% x0, t0, 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; +% 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 @@ -200,9 +208,19 @@ %% Determine control input. 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, ... @@ -231,7 +249,11 @@ us = [us, u]; feas = [feas, extra_t.feas]; - comp_times = [comp_times, extra_t.comp_time]; + 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 diff --git a/lib/sim/rollout_controller_for_multiple_resets.m b/lib/sim/rollout_controller_for_multiple_resets.m index a53301c..c39ed46 100644 --- a/lib/sim/rollout_controller_for_multiple_resets.m +++ b/lib/sim/rollout_controller_for_multiple_resets.m @@ -76,17 +76,17 @@ x0, plant_sys, control_sys, controller, T_exit,... varargin{:}, 'verbose_level', verbose_level_rollout, 't0', t0, ... 'end_event_function', reset_event_function); - end_with_event = extras_new.end_with_event; + 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_event; + 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. - if end_with_event && exclude_pre_reset + if end_with_reset && exclude_pre_reset index_last = length(ts_new)-1; else index_last = length(ts_new); @@ -96,7 +96,7 @@ xs = [xs, xs_new(:, 1:index_last)]; us = [us, us_new(:, 1:index_last)]; - if ~end_with_event + if ~end_with_reset % One step rollout terminated without hitting the reset. Terminate % the simulation here. break; From bbb52e2c25d17a804f2f594af59ff0b95f3c1d9d Mon Sep 17 00:00:00 2001 From: jason Date: Thu, 14 Oct 2021 02:18:20 -0700 Subject: [PATCH 22/49] fix bugs. --- lib/@CtrlAffineSys/CtrlAffineSys.m | 2 +- lib/@CtrlAffineSys/ctrlCbfClfQp.m | 2 +- lib/@CtrlAffineSys/ctrlCbfQp.m | 2 +- lib/@CtrlAffineSys/ctrlClfQp.m | 2 +- lib/sim/rollout_controller.m | 20 +++++++++++++------- 5 files changed, 17 insertions(+), 11 deletions(-) diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 85ab5ac..aef9625 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -185,7 +185,7 @@ 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 = reshape(obj.lg_clf_sym{i}(x), [], obj.udim)'; LgVs(i, :, :) = lg_clf_i; end end diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index ddc6a52..1a8341b 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -38,7 +38,7 @@ end if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. - verbose = 1; + verbose = 0; else verbose = kwargs.verbose; end diff --git a/lib/@CtrlAffineSys/ctrlCbfQp.m b/lib/@CtrlAffineSys/ctrlCbfQp.m index 47fb3e0..4434b22 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -34,7 +34,7 @@ end if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. - verbose = 1; + verbose = 0; else verbose = kwargs.verbose; end diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index 78267c5..baee227 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -34,7 +34,7 @@ end if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. - verbose = 1; + verbose = 0; else verbose = kwargs.verbose; end diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 6136f0a..035fcf0 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -1,9 +1,9 @@ function [xs, us, ts, extraout] = rollout_controller( ... x0, plant_sys, control_sys, controller, T, varargin) %% [xs, us, ts, extras] = rollout_controller( ... -%% x0, t0, plant_sys, control_sys, controller, T, varargin) +%% x0, plant_sys, control_sys, controller, T, varargin) %% [xs, us, ts, extras] = rollout_controller( ... -%% x0, t0, plant_sys, control_sys, controller, T, ... +%% x0, plant_sys, control_sys, controller, T, ... %% 'field1_name', field1_value, 'field2_name', field2_value) % example: % [xs, us, ts, extraout] = rollout_controller( ... @@ -38,6 +38,7 @@ % 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) @@ -89,9 +90,8 @@ if ~isfield(settings, 'u_ref') u_ref = []; else - if isa(settings.u_ref, 'function_handle') - error("Not supported in the current version."); - elseif length(settings.u_ref) ~= control_sys.udim + 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; @@ -169,6 +169,12 @@ 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.") @@ -278,12 +284,12 @@ [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 = (ts_t(end) == t0 + T) || ~isempty(t_event); + end_simulation = abs(ts_t(end) - (t0 + T)) Date: Thu, 14 Oct 2021 21:06:12 +0900 Subject: [PATCH 23/49] 1. rollout_controller_for_multiple_resets.m commented 2. run_clf_simulation_rabbit.m commented 3. plot_rabbit.m refactored 4. exit_function refactored => Need discussion --- demos/rabbit/plot_rabbit_result.m | 41 +++----- demos/rabbit/run_clf_simulation_rabbit.m | 96 ++++++++++++++----- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 1 + dynsys/@RabbitBuiltIn/exit_event.m | 18 ++++ lib/@CtrlAffineSysFL/ctrlFeedbackLinearize.m | 3 +- lib/sim/rollout_controller.m | 10 ++ .../rollout_controller_for_multiple_resets.m | 29 ++++-- 7 files changed, 136 insertions(+), 62 deletions(-) create mode 100644 dynsys/@RabbitBuiltIn/exit_event.m diff --git a/demos/rabbit/plot_rabbit_result.m b/demos/rabbit/plot_rabbit_result.m index 4a457dc..f1659cb 100644 --- a/demos/rabbit/plot_rabbit_result.m +++ b/demos/rabbit/plot_rabbit_result.m @@ -6,21 +6,11 @@ function plot_rabbit_result(xs, ts, us, extras) % us: control input of RABBIT simulation % extras: extra logs of RABBIT simulation -% Unravel extras data into one array -Vs = []; dVs_error = []; etas_zs = []; -feas = []; slacks = []; mus = []; ys = []; dys = []; Fsts = []; -for i = 1:length(extras) - Vs = [Vs; extras(i).logs.Vs]; - dVs_error = [dVs_error; extras(i).logs.dVs_error']; - etas_zs = [etas_zs; extras(i).logs.etas_zs]; - dys = [dys; extras(i).dys]; - ys = [ys; extras(i).ys]; - - feas = [feas; extras(i).feas]; - slacks = [slacks; extras(i).slacks]; - mus = [mus; extras(i).mus]; - Fsts = [Fsts; extras(i).Fsts]; -end +% 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]; @@ -53,23 +43,11 @@ function plot_rabbit_result(xs, ts, us, extras) xlabel('Time(s)'); ylabel('q_4'); title('Swing Leg Knee Angle'); -figure(5); plot(ts, Fsts(:,1)); grid on; hold on; xlabel('t'); -ylabel('FSt'); plot(ts, Fsts(:,2)); legend(['FSt(1)';'FSt(2)']); - -figure(6); -plot(ts,abs(Fsts(:,1)./Fsts(:,2)), 'color', main_color_blue, 'LineWidth', 1); grid on; hold on; -title('Contact Force Ratio ($|F_{T}$/$F_{N}|$)', 'Interpreter', 'latex'); -xlim([0 ts(end)+1]); -xlabel('Time (sec)'); -ylabel('$|F_{T}$/$F_{N}|$', 'Interpreter', 'latex'); -callPlotSettings(fig_sz, plot_pos); -%print('-f6','plot_F_ratio', '-dpng'); - -figure(7); +figure(5); plot(ts, us); grid on; hold on; title('torques'); legend('u1','u2','u3','u4'); -figure(9); +figure(6); subplot1(2,1,'Gap', subplot_gap); subplot1(1); hold on; title('Euclidian Norm of the Tracking Error'); @@ -85,5 +63,10 @@ function plot_rabbit_result(xs, ts, us, extras) 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/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 33f1a8e..5664e68 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -1,45 +1,88 @@ -%% RABBIT SIMULATION -% Wonsuhk Jung smw04015@snu.ac.kr -% This is CBF-CLF Helper Version of RABBIT Bipedal Walker. -% You can use a controller designed based on RES-CLF -close all; clear all; - -%% Animation setting -% This is only designed for enabling animation of bipedal walker -global animation_scale +%% 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 - % sim_t: overall simulation time: do not need it in this mode actually - % dt: time interval to collect data used for plot/animation + % dt: control period of RABBIT model. % nstep: number of step that RABBIT would take % params: all the system-related parameters -% verbose: speak about the CBF-CLF(1), silent(0) -% with slack: slack variable in QP(1), no slack variable in QP(0) +% 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; % Load all the parameters -dt = 0.025; % simulation interval -sim_t = 100; % simulation time -nstep = 5; % number of steps -with_slack = true; % use slack variable in QP (change this into false in order not to use it) +params = init_clf_simulation_rabbit; +dt = 0.025; +nstep = 5; +with_slack = true; verbose_level = 1; %% Step 2. Prepare System (Uncertainty control) control_sys = RabbitBuiltIn(params); plant_sys = RabbitBuiltIn(params); -%% Reflect model uncertainty here +% Reflect model uncertainty here plant_sys.params.scale = 1.5; %plant_sys.params.torso_add = 10; -% controller = @control_sys.ctrlClfQpFL; % controller clf_qp_controller = @(x, varargin) control_sys... .ctrlFeedbackLinearize(x, @control_sys.ctrlClfQpFL, varargin{:}); -reset_event_func = @plant_sys.rabbit_event; % contact detector -reset_map_func = @plant_sys.reset_map; % reset mapper +reset_event_func = @plant_sys.rabbit_event; +reset_map_func = @plant_sys.reset_map; +exit_func = @control_sys.exit_event; %% Step 3. Initialize state -% Init state. +% Initialize the state vector. q0_ini = params.legacy.q0_ini; dq0_ini = params.legacy.dq0_ini; x0 = [q0_ini;dq0_ini]; @@ -47,18 +90,21 @@ 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); + 'dt', dt, 'T_exit', 1, 'exclude_pre_reset', 1, 'exit_function', exit_func); xs = xs'; ts = ts'; %% Step 5. Plot the result -% plot_rabbit_result(xs, ts, us, extras); +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); diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index dac82a7..6908e20 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -11,6 +11,7 @@ 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; end methods function obj = RabbitBuiltIn(params) 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 Date: Tue, 19 Oct 2021 02:02:09 -0700 Subject: [PATCH 24/49] refactor weight handling (getting rid of params in default controllers for CtrlAffineSys). --- demos/run_cbf_clf_simulation_dubins_car.m | 7 ++- lib/@CtrlAffineSys/CtrlAffineSys.m | 6 +- lib/@CtrlAffineSys/ctrlCbfClfQp.m | 29 ++++------ lib/@CtrlAffineSys/ctrlCbfQp.m | 32 +++++------ lib/@CtrlAffineSys/ctrlClfQp.m | 31 +++++------ lib/@CtrlAffineSys/init_sys.m | 67 +++++++++++++++++++++++ 6 files changed, 115 insertions(+), 57 deletions(-) diff --git a/demos/run_cbf_clf_simulation_dubins_car.m b/demos/run_cbf_clf_simulation_dubins_car.m index c5a90cc..fc5e9b5 100644 --- a/demos/run_cbf_clf_simulation_dubins_car.m +++ b/demos/run_cbf_clf_simulation_dubins_car.m @@ -21,13 +21,16 @@ params.yd = 0; params.clf.rate = 0.5; -params.weight.slack = [1, 100, 100]; +params.weight_slack = 1; params.cbf.rate = 50; dubins = DubinsCar(params); -controller = @dubins.ctrlCbfClfQp; +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{:}); [xs, us, ts, extraout] = rollout_controller( ... x0, dubins, dubins, controller, sim_t); diff --git a/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index aef9625..2fc8165 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -25,6 +25,10 @@ % Number of cbf in use n_cbf + % weights used in the built-in controllers + weight_input + weight_slack + % 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 @@ -249,7 +253,7 @@ 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 = reshape(obj.lg_cbf_sym{i}(x), [], obj.udim)'; LgBs(i, :, :) = lg_cbf_i; end end diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index 1a8341b..21952cc 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -89,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); 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 @@ -110,10 +106,9 @@ 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; @@ -129,8 +124,8 @@ 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; diff --git a/lib/@CtrlAffineSys/ctrlCbfQp.m b/lib/@CtrlAffineSys/ctrlCbfQp.m index 4434b22..8d8ba0e 100644 --- a/lib/@CtrlAffineSys/ctrlCbfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfQp.m @@ -38,7 +38,14 @@ else verbose = kwargs.verbose; end - + 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 @@ -71,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 @@ -92,9 +86,9 @@ 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; @@ -117,8 +111,8 @@ 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; diff --git a/lib/@CtrlAffineSys/ctrlClfQp.m b/lib/@CtrlAffineSys/ctrlClfQp.m index baee227..3f3eebb 100644 --- a/lib/@CtrlAffineSys/ctrlClfQp.m +++ b/lib/@CtrlAffineSys/ctrlClfQp.m @@ -38,6 +38,14 @@ 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 error("Wrong size of u_ref, it should be (udim, 1) array."); @@ -71,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 @@ -92,9 +87,9 @@ 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; @@ -116,8 +111,8 @@ 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; diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 80fdc8e..722d6f5 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -167,6 +167,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); From 8e01c8806566633bfae7f52f21e22174d79ad4b2 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 19 Oct 2021 02:02:54 -0700 Subject: [PATCH 25/49] support logging of extra fields in extraoutputs of the controllers. --- lib/sim/rollout_controller.m | 30 +++++++++++++++++++ .../rollout_controller_for_multiple_resets.m | 23 ++++++++++---- 2 files changed, 47 insertions(+), 6 deletions(-) diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 035fcf0..6a8218d 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -200,6 +200,8 @@ % traces for extras, specific to feedback linearize-based controller. ys = []; mus = []; +% traces for other extras +extraout = struct; % Initialize state & time. x = x0; @@ -254,6 +256,7 @@ end 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]; @@ -327,6 +330,7 @@ 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 @@ -368,6 +372,32 @@ 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') + 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); diff --git a/lib/sim/rollout_controller_for_multiple_resets.m b/lib/sim/rollout_controller_for_multiple_resets.m index c39ed46..cf12dee 100644 --- a/lib/sim/rollout_controller_for_multiple_resets.m +++ b/lib/sim/rollout_controller_for_multiple_resets.m @@ -117,14 +117,25 @@ n_field = length(extras_field_name); for i_field = 1:n_field to_attach = extras_new.(extras_field_name{i_field}); - 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); + 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 - extras.(extras_field_name{i_field}) = to_attach; + 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 From b645c4ca5c02acc40930fc107f04aac40190583a Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Mon, 15 Nov 2021 10:36:28 +0900 Subject: [PATCH 26/49] 1. get_force supports vector input now --- dynsys/@RabbitBuiltIn/get_force.m | 65 ++++++++++++++++++------------- lib/sim/rollout_controller.m | 8 +++- 2 files changed, 45 insertions(+), 28 deletions(-) diff --git a/dynsys/@RabbitBuiltIn/get_force.m b/dynsys/@RabbitBuiltIn/get_force.m index 80e7d5e..f4f0ea1 100644 --- a/dynsys/@RabbitBuiltIn/get_force.m +++ b/dynsys/@RabbitBuiltIn/get_force.m @@ -1,32 +1,43 @@ -function [FSt_u, FSt_nu] = get_force(obj, x) +function [FSts, FSt_us, FSt_nus] = get_force(obj, xs, us) + % Consider motor dynamics later n = obj.xdim / 2; - if obj.params.mode.motor_dynamics - u_s = x(end-3 : end); - end - - q = x(1:n); - dq = x(n+1:2*n); + n_data = size(xs, 2); - %[D, C, G, B] = obj.gen_DCGB_rel(q); - %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 - 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]; - JSt = J_RightToe(q); - JSt = JSt([1,3],:); - dJSt = dJ_RightToe(q,dq); - dJSt = dJSt([1,3],:); + FSts = []; + FSt_us = []; + FSt_nus = []; - 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); + for i = 1:n_data + x = xs(:, i); + u = us(:, i); + q = x(1:n); + dq = x(n+1:2*n); + + %[D, C, G, B] = obj.gen_DCGB_rel(q); + %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 + 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]; + 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); + FSt = FSt_u * u + FSt_nu; + FSts = [FSts, FSt]; + FSt_us = [FSt_us, FSt_u]; + FSt_nus = [FSt_nus, FSt_nu]; + end end diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index e8dd2fb..2702fef 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -211,8 +211,10 @@ 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. if t == t0 && ~isempty(u0) @@ -296,13 +298,17 @@ else [ts_t, xs_t] = ode_func(@(t, x) plant_sys.dynamics(t, x, u), ... [t, t_end_t], x); + % TODO: When ODE Fails due to divergence, infinite-loop + t_ode = toc(tstart); + if t_ode > MAX_TIME + break + end end_simulation = abs(ts_t(end) - (t0 + T)) Date: Thu, 18 Nov 2021 23:21:06 -0800 Subject: [PATCH 27/49] flakes. --- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 12 ++++++------ lib/sim/rollout_controller.m | 1 + lib/sim/rollout_controller_for_multiple_resets.m | 3 +++ 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index 6908e20..19296cb 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -68,20 +68,20 @@ % RABBIT g(x) [~, g_] = obj.defineDynamics(x); end - function z_ = z(obj, x) + function zs = z(obj, xs) % zero dynamics n = obj.xdim/2; - q = x(1:n); - dq = x(n+1:2*n); + q = xs(1:n, :); + dq = xs(n+1:2*n, :); - theta = q(3) + q(4) + q(5)/2; + theta = q(3, :) + q(4, :) + q(5, :)/2; % theta_init = obj.params.legacy.theta_init; % theta_end = obj.params.legacy.theta_end; % phase = min(max((theta - theta_init)/(theta_end - theta_init))); - dtheta = dq(3) + dq(4) + dq(5)/2; + dtheta = dq(3, :) + dq(4, :) + dq(5, :)/2; - z_ = [theta dtheta]; + zs = [theta; dtheta]; end function B = cbf(obj, x) % TODO: CBF(x) diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 2702fef..417c2af 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -420,6 +420,7 @@ function print_log(t, x, u, extra_t) 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 index 4146fa2..bcda658 100644 --- a/lib/sim/rollout_controller_for_multiple_resets.m +++ b/lib/sim/rollout_controller_for_multiple_resets.m @@ -73,6 +73,7 @@ xs = []; ts = []; us = []; +index_reset = []; extras = []; %% Run Step Simulation @@ -110,6 +111,7 @@ ts = [ts, ts_new(1:index_last)]; xs = [xs, xs_new(:, 1:index_last)]; us = [us, us_new(:, 1:index_last)]; + index_reset = [index_reset; length(ts)]; if ~end_with_reset || exit_flag % One step rollout terminated without hitting the reset. Terminate @@ -120,6 +122,7 @@ 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) From a2059ceb5eff6e4c001765894552278c7699ea03 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sun, 28 Nov 2021 02:43:02 -0800 Subject: [PATCH 28/49] mexa64 files added, recompiled Signed-off-by: goldenhazard --- .../Ce_five_link_walker.mexa64 | Bin 0 -> 33232 bytes .../De_five_link_walker.mexa64 | Bin 0 -> 20944 bytes .../Ge_five_link_walker.mexa64 | Bin 0 -> 16848 bytes .../rabbit_helper_ubuntu/J_LeftToe.mexa64 | Bin 0 -> 16832 bytes .../rabbit_helper_ubuntu/J_RightToe.mexa64 | Bin 0 -> 16832 bytes .../rabbit_helper_ubuntu/dJ_LeftToe.mexa64 | Bin 0 -> 16832 bytes .../rabbit_helper_ubuntu/dJ_RightToe.mexa64 | Bin 0 -> 16840 bytes .../rabbit_helper_ubuntu/p_LeftToe.mexa64 | Bin 0 -> 16832 bytes .../rabbit_helper_ubuntu/p_RightToe.mexa64 | Bin 0 -> 16832 bytes .../rabbit/rabbit_helper_ubuntu/p_Torso.mexa64 | Bin 0 -> 16880 bytes .../rabbit_helper_ubuntu/p_q2_left.mexa64 | Bin 0 -> 16832 bytes .../rabbit_helper_ubuntu/p_q2_right.mexa64 | Bin 0 -> 16832 bytes 12 files changed, 0 insertions(+), 0 deletions(-) create mode 100755 demos/rabbit/rabbit_helper_ubuntu/Ce_five_link_walker.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/De_five_link_walker.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/Ge_five_link_walker.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/J_LeftToe.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/J_RightToe.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/dJ_LeftToe.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/dJ_RightToe.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/p_LeftToe.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/p_RightToe.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/p_Torso.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/p_q2_left.mexa64 create mode 100755 demos/rabbit/rabbit_helper_ubuntu/p_q2_right.mexa64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/Ce_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/Ce_five_link_walker.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..464b7e9c5310dd7d296362f8569da9fa1d3b4058 GIT binary patch literal 33232 zcmeHwdvKi9mFH&*2L)pKc8C)Jap+8>8IiZom1Z#i4 zbM9@m+Va%Y)Xx5;cDe7p=bqO+_uO+|pZfc5{pQP-FI(ohPl@*pkEPnMLTYV=`HsyX zsr72SQ}KPa_XWW{X};>X+{noaa;ayuOc^VfwdLaUM^VLdsjDZQ$ZOGhuPAL_u5gzY z(tQF|lY6y(a^J#q(9#Xr)X}j>x@k){ZRuR9BE-j{;uyn;~ zxutWd*aEOEFZb^JkH~AZbp5T$xH!GQ%5$lc_bt%TF8|(->*g!1yc6|9wQaXc+28z7 zE_u*rF27c`NKcD@aH85PF?%Gzy2469RGb8K#}9YqT^!lPT+oe5&YH@z&YF& z!~fS4z}cP%{uh640bnsc?I(Z_p8)<9_c6fXHFK_E@(Bujv8+$f%_K_R2 z>pS;$?P=>LL49|3L(jDvch+_6+}hu(_>J92*LSzI_O>ZeLu+sM?*8Tr&#(4+c6aRP z>>=f@-Fu+#pY`qTZrj<~wfp+E?j8!>v-_I8{Z&1kl4;B50IB+#x4sS9Ufc(&1nY%y9QS@0`@8YP7Uo*C&r)->b@G%Sb z2gud)9Nd+szpHSLNy}OQxdOOr3oB*{;C6};{MiEdsRGfjvH?wrTWBR6`JP8OV~G1y z4rq>WnWt<;bpf2Zx=+o3=7{gwMo8@dzm-1>m;XcuG)w$b@=(ufEP#Ki0KSx6vcQrB zmMpMjfh7wpSzyTm|F10YcIXQqMsI$-GONB+!}p-qKov%wR6P;MwH*B6!76_wW(mDd!Ni}|lEs$b>G z(fGPEkdMY!e45GEXM0z{lz-Wwru^jGbm;5>Xzv}eDe#Z5GQRc&Cg&WB9-N8Z_QoaA z+h$9m%MM2$I@bFcFxYMxl+I1>3Z0F1I(piBU|kB;Uf)-?MQ>hrCbQ9lFZX^bdh5Dd zP{=)bY;G>M6UI8cVjs%Owu5#7{gE3`Y4st$Z6L3^s_xr$+v>J%JowI*=)rrV-+SFh zFWmg$@=)xY%aEB{7W$!wO!U@^b3+h-bmPH0bN9RK;aoSe`?I}<7`I41wEut9OH#2pHfXs4m_d z@0-{h-$RDi>nfkKL2ajz& z`2NO&cjTV^7_gApmQ@h4+b0Akax@f97?=&|_VL`m5N|`sv^hlNw<)FpJe-OW6`4dW zGP&<7+wJ3dww9OKH$gUn1z~%$ED7yfs$M|@*6yPq@V=eUr-<_y zaNdV*nHnqhaqu;3n?xi3AxgTROsCnh2WjSNAc7wWA4NGei;g1N%jEewsDm7HFH*m4 z11!IIGz3ne$_IcRAE2@8L;mXk90g$82pGLZqUiOkSp?f4oT==1W&RLZQ^HZJ3SCk$L?PTXDO{X#{_w%m5`;Dr`+my(c(K zjC4Phgmlq(5(UV)A41uSqhq2#IN7g;m;$1yuS6T4L~U{qwYjw)QAg7@?@`;cY=e@* z7s2m!p{u5vW0rMtM%(sV0Jn*=JPCFDXQ)#$OU6%Ab$H?gzUsvt6GDSNYsiA>$AI?< zTs_$=< z_J@;QwtEi|yt-Y%!PDT0W(O~WXUwNg;E47>2>8DQ70^-OpPFFvoXEp8&mDxvO9_YE zhxqjbL5V0`8Py+V8qBj}PGH1siC!`or( zV2TKg2f?GvJ&hb<7{5m+tg>`X85PMaA*f2^)H>nh^zXIp#4)2X&QKqp0+P#9kI_={r9v4-OxqB?5lkp#+Dc1KDmQeBWn1x&mrE!>H&QI41}**!yGb>X z00JhdaWJ7YjBJcsatxWIjRViIUmhWV!<>TerFIXIX`BvA5ZynVU<@W3cDp}GI=TbA z7?rVUqQadKYl4)F3owX@ha$7`Bq|HMZS*Xo-VNGmauM&JVXbyQ8X%=qyA{vql#Zz0 zYkz^+pKTC zHCXgrlDtM~t1lxb1P6*>wDlp!tgW>CM@J|StT~ZjvH?XVsz${)W2QdT^c=w9_%Ja= zECkJ1L1b$%AOL@dY%ueL(_`9*^cZas3epp5lW<~?je+$f)^L*aWpKu%8y)K*rCyuB zs)gKjzDMobEUmtUU`P@t_y~f1gCSgZ6I*#fH3z2~mT`~BG_~a8#RH{i$!j4V< zV}di4P>KAg?p5pdV6944;z*ems*BPrFq<-#0Z6n@G=!zgY@0OV09z*OM56_ZiWKl6 zIej29BUa{zD4A7JD|42*virm70VNJ*L^ERbad3w-2iP&-Q{W{iJwec`LK!7%7#<;0 zj0L9y*LTTc)HUabU>cPX9`&p?uM_h%N}M9XXpTM(I$d!gqk#x!T3x?h{UV)HCT4U- zGl6J#FUgV;fzb!-7C$M0)ip3aWqCJ@Q&f^{sI_=`+QqxK*eqHzz{(-WNK^^=nQ0Xj zPIuW@-^ZCx!6CEm$xESIsA*r8EqR43USUo>^iftq9T_MLcj2@s27UX+SWLBZM#AuY zh2u#m8BtBTg`JN2mId{!TKj!6g&fS>;gnCMLZ;%si5ln?sPt{k@5^GAc@?whhiK04 z*x6t0=uZQPUczB>NO~lUA~XQnGxr2($S!k!IB=cMiEDNpUt5dpp%1bx!K3{ zfR5VqfE^>E&D!7Aig5#3^@TIlmP(?NEG06Plq+h6f``eURyAp9I>3}`;uF*!B~EE3 z#W2N^%=^CwD4ZTv?E{ti=ah=D>*9EPh^>IO4^qHA%sna$NE;s}bMRfcmSo^7>#;;B zq!3dg2l@IB2>OskPjT{wl+|C*Hik3(XgVqGBNL6Nm5LmW_gkv?ATy}XuTV|JW0$Dh z7pR2>dmub4(%Q9QX5~1Mpl+(gLQ)casV-$cMB2D*u>S~s^lAF=b8z}(gfbZgtTqYj z4JI*bwV9-Q$~rsIVnI(}Jjr@PcwvRU`u=Z-3Vy;#>m3d*+>qLZR z50NXwnTgft08X+e$xvlUPeMzXvJzcn>-1Rlpk*>iJy=DuC)p-$JIu--sy5j2X&?&j zRfLdlmnI++^JB`qRdKOr0BMkNu@&o4om2x^=yF}qHvo)qtdcw-N@k)&L?xJQ!z9PL zE1Zt97vQHd8(GBsayN(>wNNIqbn$M41N$_P~BN`-Y2tjuVDT>`lRlWSPJ+=XRi5i(1JjBUTkl&Xyo1A(At5>{Wc zwpkemx<&<=WGGioFeGfQyGW5RPg#d}k23S&CS%xokd{em)}CVVwF0Ob?V&oQo74;y zLS*u30_cQ&TR2^6i66Td1|ljON9%L`@?}$t!HCfm@)L~5Oh=dkL+1!E5nIBic(RaS z9KbAwV~wj;eU4nwBkz+nj|s$uUZC_B0YW^-tUe@c!*-T8>%L5)P=Gm;ZRoNcW~X64 zZncOzEkb5oGP^zOU1eCe0lY0D;{V`j1f| z?mH4H$*kHWn1OCYv1w5)$oUw#TGZW%DSaNAgCmg8=$^6J_A)JD8nlY(%x+<8{Z`fiU)|%B9kYT}q zqe2yDs}x-ZqRSqk0bio6aGYfl62cGKNCH8IJ=IgrgH00Jx9(021P*G#D&D$=VfXQ3DE+mVtjgw{>Qvz@hft zY^byaNC^owsLn5Wc>ome($?tSG&aZ*S!XW*YX~|_ubU=F4^04?rnxxto7lKECx}f_ zPY7}DB8zhz*LTHRoIZl%2LM`)WZVz2o3>02@nk! zxhB9OkX`<0RKP)g4_|R6TLH7rDmRme*vzOQDhRm=5NigbS=M1vt5N2e1KQv5YAx%X z1KU_j0MwP;PpS-L^j+CNu7<7C%Q)W4g5%M8Bg$qge8R|+=M-8|)GZlYGmb9uz5$=H^|>FjLOKRxNn{(;)1ly&LCwh#nc2+YANTo+$|vr zIc^d@h%{*-xNzqX9oPV(}{;Jy)BUavCHCZPlu;+p2`CN)6kpd@E8Cs;R&M zN`y@q)fxf%^68nxv1o0oinu64mC?gNii65?cw)j~>E zHQFjJ$}l9M=aIZxh%MkEYjIQ(BRpEGgl5eRlnE6p9j6fnX4nIR8(iejM@ed|2+@up zBl}be6sc2W93OV%gOo1^VY)9}h&E)=*1Z2;q;t)x4x15&Vk&sP_SjA!z2NiK8bSyrGp-* zTfIRg2YNk$`qZFpjoiNqMX8fCV7=;D0ecQCqk@fMOXZ3}PR5h7I`f2MGj) zh;fGw;rN8MIKPLXf9m}_R(2*GQKMo)7v8Fd&JEayu+r(41p;*wJFGfSb)&&-l!t!f zDhq+n3e}(i6zm&KM#dxN5~`jUb|_*Hpm2?p86p&@)}nHrniyi6&>e{`gei9QQ~Y8y z$P8+8vC9Mp>3q3wmO+S%S*+T`G`M@j?GZ{kI-?>boE}tZcGnwVV}Xf^XIzlwOF!XM z0yEB?Uy;5{jcDJ+4k%sBHYg_D0DsW=TemkFghS!jfJGj#eU*g-+&v5DRKHaESj#~w z^gD!1Oy_Ptrc*W!UNC1&K#y3XVk3_SBCyxT3WcXRcft~Ja$@&rn5tFGdw{{4Y{)b5 z8p2@uxUf-Scos#We0qRt!RLiE*n~|DM29LT$rXBum;@P9A6V3^H)NL-zVH$~t!Y<& zyp{E-^!f-$?;sDTtdL4TU}F)tshSQ(WD!2W6+~Sqyt%xTx@2wfq;iyEGUMs&9aMe%5jSV`BRxc#Ns;N)iYv2(4Ni!ef!TPThdX9!f)e-n(*A0l&2hem<2_khq81Y5Bp{`WOR?OFUe!b|gkdNw z-~G6(jVZTZ!b$0UOp~(Ml{MkwTzsV#Ron+e}z?#zN-WBtu98l

-cLk{3nF@DqSh8t)i%SYhn|C> zZo;@n-|%Efl)x7{Njx3#4gz=t!xKiCMuJmO3Xe*d0QfovB!o%u1UsxG@#q3gLfeU| z)VY=|QC|Y3xHPcmnq#8*dzbZG+8FdV)QrjC2Kpo$2~61% z_P}a*bkC|ODp#^=>#&_ULs zrk>#tu7Uxsc9pRN|BArggr^J{pk8(qGRASRX4i^wSpuUya{2q52rN^VO_i$P{L&wb zDl0)ATA&rUIRh!avXBsy{VBjWQ}AP@>YgTT>{#^(K*fxvoYqsG=;o(p34>$JfR}id zpRUv@X_~BhK$;^OT8_SuEvD>_g7eh^GBxUBU_b#Ko|aM`u|aK8Y!WHVG7Du9DMA6x z$9ee@-ZuisO@>W^?!iZ#7o;Bm4{oYv1F0F#^AR^hXJFz9IA5@HAY5dof`Vh@=IT;hTd~ztDi8s38N;z3PFGvO;@uQs zZ6)#QBaTo7(gflO^~11R?0}kphaG;bOXq{w08@y)gRK)g2yJFUyB3iW*8KDG8cN9s z5y~6N*#~CDgI|>SH;{+c*g`&P89b}~@5lPVn~e}>)bmc0o>Es=uR_oJ@*EHX8}2ND z%k``__&FA5$yGx;PeP@Vo+REVY9qSg2#4B@69KTh;8HV6#}b0Goq;o||a1-YqHq#g2O|&sBYI&~@S%Ct+2 zlld5KZS>g{7>p4NHmwCLHevfidqLK7(CSsH_7U18Pid+>a@1I7ci}dJGTWJd&JygDKt>$<12K_BF8>b}qP7izl?uQDQnfInJU}n-4d|qIUJ@hOh!%^Pk)vr? z?$vo=_IN39w*gHET}?Wmpi9N_vt{Miq-zQ*GPtgi-U&oK|G~iTXN%E3PDk(JKV~YF zbT2KlBAS5vlRS)eWwW-(%=|d@ktt9qtir(-x}#jcAQnc6Zx8nEgVfMaFXrKg_H@TS z{)jc;0Py|PB+Ui)`1q8#^CX*mLe?a z^_x}GA04)W6g;b@^!4N)yx*|2dORzmvJu)~-vNKm(ifm4&tZ8v0PYVX8zjbA0Xw>5 zu8gigWa*`Y2KTTg>1ktcXOBG$L6i8e&jO;o2Uv+7dxFf@9saxq){L*SP@s=~!+x1| z+YcGvWg3~$min0i*o__y0VW}R0U_3y_zMgfU2;FYg;wJw>n5=p_9j}ArHnrQ)35Q0 zd$sV^a-SUSqUB4F_Xsrg@gNz*STouS2AmSGMwv!OI+B&dcVZIt|HxGF9z5|G} z*0(v9u}SgcGZw(%;mbt^w7j1##=}^X=of?OB2;fKLSf!ZoA?3kDXb1n#s5A7!^)oR z$+qQyl5xY0eGC{ijL|&tVYQQgHzkNwq&8klU5QYs<5D+21JVNAXgMm%?NZk9EGr%R zO{JXoFfa2gAdr#AQ3u289Xakiw1E2%iSU?$*Z@?gWjYH5sWH{TB!<)`CY}>fGJM(Q zU=w>+SknLLc{+(IP%d_%5w>4lR8+z|OgteS>c=I}qdcx<@IidEbz>^hm+0cNAT0y- z2nz^~57CNo7o_4XmOyW3*lZB6#fe6(!Sf|*H3lU_KSa11)&L`_DIeNVl3D^#z)Xq# zh}7c283a4I7UAiNJf$C&$Am=>w{qbW0V8NXWu$j;uw3>zaR+@82~H$kh2A}1y4d=L zJR3GcZu%EUkS{0SE1&wT#^|^#>*y4d!ryrsXx zv!e|CMk&&ZtJ*C!cgl1cn3*kBRGH;opcQc7ZuB|~2y&|gZw>``2!dohBoQ)`wl@<( zUvVm#bA<`ll1;j7%=VW^ha$R>0712>TCSp72<42uW=i~};&D0#4j9xje829-@A z02Dt9u6*vv&lvTejHhlB{&D$$$M%?2H?UnW$_}^%_Kea1KQp5$aSUR*0vAqvO`Ob3 zI?Kof3hZrl_LN@WR@*-Rh`O{a5YR8_Qk95jQ&5j5S5WyX8!Kgfm*dr^RIK>+XJ8;vk!}#RrGl9Yb-D{_)Y01%6CZ(;FHscdj{t|?(1@o| zbZ7uXLz!Aer|dY;$Hahk2lpP_LEw=Ptqbd8C2+_6<%6^l5cB}OmNKCM7nDsg@jr0q zL05ncGQQR23zXn2~= z;ObJU^#;+`e(`iMn$IWn%(^XF(&tk3KmeMZMMB+U4A{7*4_`yP2ALt1)s&VI0uosP z|AsuTmdz$_paUFKfr5WOfD0SgtdzqGF8dt$zfI)ily%u>01E$e`7m20okCq#;Wwwq z2Y=N8zm%tm^;!e4Q|G%P=X_*RmBn+^bZv;1!!T@Bkma5is(_DNT;4`1iu7Jz-_0%j^LDY59mW0)N?IZAWK=f8w{1s6)d5 zi!revmWac&qy$XVy-VSctLJqRG@qqR^{7hA5A(BKby8Uk^m;T2tz}u$MeRVR-mfBr z=jz+>6HlnY(I$rV{_-)k_o-~EL?S3b_Be@SbEFGVkl=*W1|fIF%Y_9-5d=vfU)e_H z(gL<{!ug}oWe7J@W_6w7*hBFMEt2JzP8+g=6lU>Cl0#E5y8ML?pDc!tj8kW60;Htu z{p%+5B0>r+1a)u_i!SAT6exaBp4C<&ESk8n_NEQ772|AUC$N*PX&z#nx@W!})7C}r z1gJ?al_v`n-gwvk@=5ULhxsYZQbW5~8&@SJP(eDG<>OKIkPQczECIh-?htf%wM#9Y zU_`{_T@G|K`WOd#gl>W3YBU}reWzicJg_i*Zhl|ah};IKHt z7go_&w1ED{wUj< z%t>`j8*lohFL(!vDFIsitbC@h74m=-bw3@dV_QNSvA8}$L%Sakqm3A2jr{clYf6`a z3mHL|+X*lZfXfY7CswQGmHlpx+XovNI4g&LSw2hJP*6fY5TA{&6lTG12zdk?R^LPj zMiBuL+6I+na!d>_w+Ui9eI^Uq#k*u}F@I#M_f7EQDM-~n2*WB?)}FH03Uph|b3E+- zhEXN=wAg82l=CbvZFjy>FV8-_M^21+sJ8)ZeKiOfKI{OK;LLS{hEuM+M1Z{ux0^bc z;iu(guo?+VAS*e+_Mre@#cHS0QAO0x;M0&Srvf@s#OfbE;LsVpV1jvKdbD|?oJ=FkwRhtW`x z&n#%fl90No8XG|l0$HV%l3D|>E}tstJk{2Dn6H_4B;E$*|Gq{BlW<(wir=H75bkUe z2gnh|++SYFmf`}Tn)43-V#}<&keCoBhsh7pjj?5rc}luZS9hFuKwfJNC5!&+#&9m-lxC52cb2@1&szzF4%=X*z%MKr$zX}&Ix~(t)78PO@W}he_Y-uMhLzFkkPYj zXwKy=F07;{xnh8`kAOXv5#SQh*o)vJ?2N9rA@X%J5DRfLI-+u}gWF+h1}9$tKEgl# zeilEv2VQToJwGk?RXiuV)fce_(&ZRLBP4EUF(1ov3Ie#JNle7ScQ8c=;Qe`Sa;`3<7{uMSC5mcEuOuw;QH3oKb+ z$pT9j_)oIH)*Ct_yF0r2dMkU*iMU^>j#OUX+I{X<@paxgRVuilZtLc{OD}5J(%#zD zw)3Lh9oOThw0B<7w|66nd%WZ70llKL_sX``J>PEI(c9VWZSCxg>}~D1G1A%Bi^@oA z_qBa{+d6uCs)~vCw)XF7@A0TZWOq;GhVIUeYpYm1ub{Zl9s{oUlfUNYjN0?%^OagH zf6|uEs#Uz1OBDX(B{zNM=a)bKotG{d|3d7wZ;e0T(qCKptfpsW&uRMM?eA9p($bxg zwS2S6Z~BsNpSI?ol5f6x$%lXOr@!p|tDGzU+LnKP$`gOvweFP9WfkxJOO~Bgaxwmp zNPb&-snlj19W@QDpKpIq|4>Q8|l8?n0}&!6Fc z8FaQk`Hj-jXUPIf7Fe>tk_DD5uw;QH3oKb+$pT9j_|LF_d;gAm-;O;n_MWoBT=~VK z^6zZRSv8Hd91iSRe$@xfudeEz^HjxC0K%4}-U@D48pShil)*1P=@4x{CRN2cVHvw|GF z)|Q=~+;{QwNliQ_54R4Ye{O|V;7UQtPA;x>q#UvQaCsqRXZIOfzTMK}j#29K?XdFx ze99@x0cCjk`t=t@Dz{zJ*U{S-`Fhp)Rn=?G zy|7P;=l;cc)m7COoTK0(`HK}xJiMdI8M*+W1aGf$@5w6w<2_F$c|ThSKgo0cwGh6- zbAGrG{t3_Z*FyNop7Vo+@KVq9|3dgDJ?Cc&;iq`c9~Z*yhPn`le2ModFJkRnSXF{I z*o`d;Kh4W73J>Mq^SF?Y@!WV{2*-}Tr4XX|61=p{y*IG1w!}N#8*43uIQ-9eE`Ai& zmUyc?7dIBdKkF^#rzPI!JonzPh4>h)1BDRHmw0D*&MymVOT06^mPPShbeqTKI6i5S z_t!CQF87YNCmwYWcvsP3$QLc##c34fgI{rim(T0t;>KkHKi)oj0jFFS_k{_*ZWR3E z?K}kdV*3BX3E(Gc?_uG8y0`NhWQsnE@qbis&N$A_zX5!?cf7wn5BRc0`s)qhbEelm zAesEv+ZJBjk8^-8^N#nQQZelDetQ<+lv~W_j|6_aU#$atIo^hfCM@`@2mI6Mm*V#y zZU;QF08z+Zf@=FZoRP?=hWRddb_$?_qH|f z?AyEdMvxSgnt|E70Clk!IPTct?da-i=6f8wb~oGiKvn?+W|v)A*HGVFf5isA;}Y+Q z>}~Z#K2_`KY;JGu*vU6swsq_T`w8Est`8 z>hK*8S9a;&7tv>Jg-Zth#f3UP4o^$-We0x`74%&v-|c(c`{!N5?D0XK zpR?o7K+E>$>A#BtKGSl_;CT@#&3`$$F4gt&JiRwyal172=Sd~!=<+Fq7s_}0E#J~R zJjbTEd>V)ebNaUoC}ztE=qao4bGnsZT?j$ngd%b4X6ft*l9WICIV)Vc3SB%dW5fl}@{|gbYj#U5v literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper_ubuntu/De_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/De_five_link_walker.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..b7e2fe6ec2da8d6af140c4a9093107c9111d8f19 GIT binary patch literal 20944 zcmeHPe{j@AmhU7W5rz4Viifc3+?LkH-D4`KDC=%aLI?~ifgnU@PO`(03?yqNiIW)( zUa=P|#)4zBX3L`vwWwmx<%+F4b~!CA?$q7zLqNBbs3^GZY7mu)3dAUiip+gJ-Tfvr zlf=5(+PeE^I+gG1_xkl$4L6ObZJS_70+nNM+8EEO#k1irE;4)qrJZuxUa@IcS+FeM&;_Q;ork8r z64TySl?i*S+$IY}jwt2&tz5sA6R3pW!a}1~`$#{xS$a0~SU!Q-atEwjHoVHp3DmIw zZRvZl`oF|pwUw)$X%uI}>#aS3Vs8oL7?=NTkzu~o+8b*eifz9IN@mh7bueesPiA2kChZY(XIhao&E9PKf62t z!f19I#-MK*gZ_7*d+?uKHi0mj{$5RYsn%~B3A>!Ai{PkReVjDZ;cJcM7iB+@gb4;$ zM_QVLZLv@^77VK3f@M{~`fxP7y0I-5jxMXJXliK=FAJ?~3Y)gkTY`1%q2Q{<=1^1P zyOQf|a+)jbYXtb(r^@95H=K5vr zF~eUS1+0jML$Ro}ZIoQq*aUy?iZ@2X^`X|ryTj2o8g6P_ z8EN;nwdhJXRRhI)gPI$Lw|9qwO^wZW2JZ*w8sAt!hn3q|v1>Y#2Bs?+Ef zrqOfNTm*6v$VDI*fm{S~5y(a0uN8q$TvvSQ>-?zDw_*IzQl)%dy|MgcpRaRo;T~-w zIqMyehmuzx18`lb$lt)0p@E~xWU@!&+^`J|yqv&J{P+L3Tmx_!esv{*Olm}$TQv@C0o&~;@_+Bb(TzH)wlk=*oD3gB@X~e zym2m>Ow=P-`^K*Yo>vRm4Ece3&}jXkz8Z*^-&S6;VEyUxWlMeQ|KPjtqhgF{=a+e| zuJ1XhB=cMk=A+`<@Pou7P!G6Z{oceusqRa}P`$r5=8$8ZuDkBv;?f;WTuxAO(G^O~ z>`iPiG7G!oeTm~v3w5%6!TL&cY5&6R$PtYXmAjm|S2+vUf3|S_Dc|}% zi5tK)7_*_n{W%Gb7z7v<)-bCVSt*y3q zSX;Oc>e=pukF0VP?IB~&s!CVUS!xdV;u<&yVtCV#E3lWkwy^au=*#N+7twuiW$g~s z+-n&?|6XW03Jb?bs_dZHKFtUE*;VDPqW&S5|2QjW!SxlSw>BWN;8=JQmF^v?bOrWM zEJ2jA%TH0kUdo)K*6k}O0H?mj#h)ZEBdnFJ-DcR(`Xw8VqrriHl`eljWw-hv%Z9zY zyiXG!Q&#uoG;03+WUJI8vyaU~bm(JpC$t>7pQFB$Dd8cCY;UJ#`bf4xs`?zgT8pFz zFmUcCtH}V3r*XM`*6Qg>k@Izu25Dr7=ssRP2mf+90>e5|5byHYUhCMOGOFo@=-=LI z^`D?%g0cA;;eED@au_$-*>BV#I!Q30uP&$L|%3~lc12RJ9GC}d?nG$NWE{+y*h?~wJ`Vs;mb^~f8H8fKFWGYCoG1Y@H` zjsbgwLQ2xYqycWZo0QB!|Q(5v6YQyYz)Jk4S`oFuwCbrwhO^-ohhgcP{YPS%fFF% zMa75N#g};DpgUXbpt!sH!O6aGCOO>=VlZJ81U_dQ$Jo(xgx3E63=0MuUS>m82dS(E zx^?$q*_nB@xlV1EAj1UIz4PHLxd;yi|Mh7U=MD`4HMR z>coKJL)7;!{o^es-pihIS>;`Temhfk8ELJtSi2cG1WS+!T)mlXm{W&%p_tRvu36k0 z849SyJrc{#TB9S}2Nw?EzaPR}Mc>Ci7y@x3;}?Sqjeo-bJFt$K?9_J9zxUZiF3GUr zax@=Qn}r`OTqT^EEsPUq|1SJLuMMG%mR_|j{yys24a$B}o`Ve_j!8uj4wFDZ5OGHb zCI^?6!RmOnGVw1MYN9WegBY#x{V>gR@y@I28YxE=PJq zn+cY7U=Mx#Gx+tmzYPv8sHZLj->`(*b(Y1?`>7MhUI9uC^W`^d`vDBLyGy4D_7S~w z1gZl2(L?Meaa)3+&p`MTVj9tREA%LU41ro?kjIR-R>W*#rwR>?VXQ&!8d2t+&h6tI zuLNx2E{3VLz08EbiFDBZhqP($ANcqgA;#Ur<;Und;~xeAZZSJ+kQ4q-$@@On6Wnx> zlR9JEPfAP&+02}9%xdNe=sPZRZYyUIR3VAnPwI8X<;=QV$4} z*cs^F%0N>!^P3BCfOl?86Odj<&584#iU?sz+06^X@9xqQ1FcU42c)jD_w@ar7Vw|HvQ@zlGT-4viLoo%nuG8T%!dyFjOe1uJdsGOkvK5{dN=&duQ z{$kV1UYh5IbaSg2w#^M@*tlM@`@Ba6xC?Vv6m!m_yZ%AM43o#HLNW)QrxoNr)<+Bo z)9V$m8~}^X$7-sB4Q3SP4b}M}>+j+k3aHmoLcMmHA+Fp4Mhq>cAA$!>uR`@^ombEV z#C^w9QaF4>JVbP4$&N4}H{Ev>(TdgzA>NRk?Oe~dhb+XYNlLeOu9|lJ95u;FI@quS%h8g$N2phZ9lPv6-G~6b>HEm z%k97pa`?EQ*617KhlYVM<941BoD5{b-NZ@VxUnmA-%?GA8A$gn4G7!|jE27T@W$m& zpxq>wjw3=+y8|lqV$N~qzSA(Zo6uIYLSlR=zKqcUE5p^8D>XheZd= z5f&GkeUr?a^)w_Eoqlwk?fs)WSwTN&(0SI*E%(zsIw%|uE@qta?#>QPbIcwX_%@5{ z1q1jFgGZ1rj=h;UU3wL?8RFaD*#{mLxOW2R5rYxMh`T!v=qC50{Te!UD!4n(7+g?&M(Wz z_{0h$h25@xRCv0~Fg($289M-d_w8kG?kHbgUR(ZS{kgz`&a)HZ6Bl%z%X8fy9>k+} z=UF`T&(pPY6I}N%L2ckhTvDH3%sP(m$?-h@6#$;+pFRnOcTaKM<>wUmVB^=OJ%6%p zf0XTeQ|7o9hf97l|LDE%mhQZw>%%2G zUlREH>vWl3X86u8yJq==yH6KAQ#$eRW343VF>J@v{__t?T;xSD!gHvFvBBFL`a;k}pbs=sNk*A0wZX=1nj7!S&$aPa68( zN+zoTSG=7}wga{T_5kkaPbT@Phv(yDQa-xChm^YH-FvIbYj^UdUpS$#2hZwPfy&?N zwnG%|@_77K{or%RXA9}e@qYyL4of58@3~{F zz!&{n>&nT_1Njw`&YS!5g6?tiem1`Iyn+v06XC^N{WDtX{1yJo@KJ#MDKDjSB^QBQ z1ac9`MIaY}Tm*6v$VDI*fm{UsA0r^|-^u%Sc!bd9j}{7kEDJwj@zgtK_8|h_<>_+2 ze#%e^zf{vj-uuHZA9X3T5Wl(8Mc)78vkQx~pFWptAwFnd=972pwu~dyypv1l;cv!K z#s3nW9}QT z1q$z|N}$s?3-I=;yeFSVMs5_O@+^}+E|p)I^zlmaFq3|slJUxUWgP_e?sT{EO2mre1)T zw&mP_^wt7(v067Xog(~~C|N(!TMN__CF@2e{Zchro)%y>%X`C_{FtrP=@e5h!0IJ= znciBUrl}R9^2_Qr%*}Z`Zj|@exo+mGk@3WjAT+%->qC>jwRBmhf$DN2u6b$x$ht94 z(?`Z90y^!=y04YsvPScdjPoYYNAv%;W6;Oy?>?>nV)esoQOPQ!=|7}-E*lZ&H$l%w zl4s}JQPA^78LyLC&otGLHC~@sdiFRbwVsjrQ>X(wGH<7YPP^H9zN6_Q^Qr{&e4K`o zJx?n@zX;=!ef~i$=pOBB=A~&2`Z}#Alb6^-W6&QNgZ^)0(El5B&q(#^`7!w41bwvg z8{XFZuGC==`ZGo7{csFDLu1e{!1_L#KUaf3+Bp-dE*fi##aFHJ)~VrRA%d|;u#N{q zw5edQz9qQ2sbyuTDOiusliPx!c)P082Rp>V_1@XVHx!S~#PcE=gP~|Nv?hqpyrXN> zs%R(@4%Wvbku?xW!-C+9Ww4HRfJ0rKs%vcx@*IcO#-Ke1!V40V&AYX{sxnx)XfDsV zz&R1Ikka<)T3bu7A=F&Y6E4Ed^`NY5YcmI8sNmedqVlQ*6(Hi&lVGLKD)P;}6(pRF zAu8uDTs)_IVQ}%hc}pvo1(%i2Sy%}}bo(2Qs0d~roAFI&Z)6;tVNcW;K0m`8hEZ0Q zer`q9!5)Z8))5;#=mONtlR9`r#~3GWX!k}v{zISffx|kKw{1-%7Fr1yiyGJw1%UxHOiYJt&BG|)z4_GSDK+A)YhQ9^=q1;%)nUGw8+sOn$03XD*iKKg+ZBp&}=0cV@}UzD%He9w7YL@%x>Xm+?C#WZO6U z%HjNx=L&(>rwgN=ZJ+y0nh^W){2`G2OD~c?LVF7p?%CyeT%J?pys~V5!3BN*S?q#R z^71@$#GW4~d|=TGZ#|NqWmpJ|kvh06KelZY~e*4X}M`Obm zu`AH*mxtw3&rxMY#y>2oYa*9|FrIE-p11r~Uid^!Ha;1`q>2Bs4_`5cJZ+_yi>-Z| zZmA}b|89z6Dk7f_C3n(Ud&&$}Y!?>1)2^<=zxdC!h=rdQiSL?VFTL|jWk!~Au`9fs So}VE)V3CQXI literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper_ubuntu/Ge_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/Ge_five_link_walker.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..0f11a107b34a78b9a96dc9078d75cf9e00f521a6 GIT binary patch literal 16848 zcmeHOeQ;FO6~CJRiG1zyW%&?U9T(=XcLJ_q=oOdwXx*yFc<)R~Fc8f=i*eO_1asV?5Vu$;M&LY6 zTq|{l3{(x`_7s^AQ{gaWYGWv|O19F+hnadzxt{D~drq@nSm*^(Kw+A0M=pCyWwq#C zoJqS1OnJAWY)2y5om6%wl^s*T`id5tZe)xU=N^SysZ;4Owb~s~c2+uF*)f&50Ji#l zBk)byTcPa4b;_}Y&Q&l|Zf_;*R2M0YuP5Bit5kcr`k_o+H&fM}GN0sw#>}$ib(HoV ziltCBOZ<2xQ~kAT?h~a?J^klRCBIvD`6n|b-hFoBFG!X2N8up96u-j_uHD%VR^mi^ zAG%?|(}g=m%=_`9e;_$qsRZcEQb93h!y~}QX3_WOz_;YU|Cj^ERb*{0(9FcKD_ zwq?3eW0K_{Rk4aloV*cR9tpL!=xt=+jYex?8>#}HaG z_sKOlJ~P!&BcNv!dsdqF5W&<>RLJf_P`-cTlY>`LJ6EfEevb0<>w@a&6Y4q1_+EwA zyUD8%jI(@liHXxasa#8-FAe8wp^6J>xcZ5a`k$xaBP3#;Cb~^Zy6IVRrr|u~NV6mj zm%md4qLjHM5#)zr<+9Lia)k4>5#n~!Sx9{(E>(PPlO_G(DO89RY52%AJkRDckk3Fq z1NjW(Gmy_fJ_FxY1}@sK{leFAuGqJ8=&759@IBCD6eN3n9S4gK$R?8WP6JLPXPg1C zPZOlyOf`xAQ^{nqOUg81OY|R2$uv<*^zTc_G~r70Ka-MaqLt`>JSEcvE7AXON~Vcb zqW{M!nI=hz{(D(QG&5v06+*@XxyB-UE%IWEJkKJRT4byLsh0W)EcsUOQSYPM+#z^1J#oHnDy>%s->2!{4&Y>ZU{F41>6=?>%pEM`TlYutWbpxvJ zAsp`NrK?l2i~os5AHu!sH5i{J<1W(}nn&q^YLK1!)711ERB;??38+nY4R#3XD2~73 zI1a5hQP+>Unr;BdrM><530!*jpkdfGyov+Euff|xDY*>pHEQ53Fz=HY z0ZSo@eupV_l0|{45A= z@u$9k!qG32N%OgdN5G?IeZs8YN%ha9>goA&xM)g2fjEq(epwOy>JTP>lTyDzGPL|D zpMiV^@)^iyAfJJJ2J#umXW(1Nz#U=T84S0^jgr_*C%>g~mTdG#XU%kp+DOD1@`pD& zBXI*dPJeVmJfw$>m`l`nYO6g<7uBq8_P6SRMInD%OLI(+oiiA7Zi+_28)#)}Q45AQ z;uTzAc|24_jm8Go16>|5R_XqhJM~5*63u>%?F=6~^V$5b8m78U`NERN9;NaY)0gfW z|Map`Ti#nTyZE=){2;KzJ?(+BD_{6k%>8m`Zu|4@@AcTG6y8ejw&`CG&X1DG6`*@T z+dz9jyFl@}GPwt&{ZcY{f@BnWhepM=WtFhCIc!r#78Q44Ejt|;y<0nSUfGUzRF0nb zefx+_#qDCr+IHI^6YYFpx-4meQpCps~?v?O-$!8#+fqVw?8OUcKpMiV^ z@)^iyAfJK%pA7K#cl^B_e@pidb%`vWvB>m|9~Hb`F^gK1;EkCqKUd#zPEs;|_cuk! z{LR~BCG+=xv=*Ym^_Q+BBP3s-cf=_CZC&>eQ|52#obr{?z_)SNtH9;r2C5TH^?`5s z+$NyhiXK(Z>oj`9NaZ^w5q+v5#@$NhIBC5{1urdR;r8(6L&o2y0>_&-DRaAc>nLTX z@`u-RQs%s4`6lK4%Kkq}R+p<<``?k&ckGrX%T)in6iZqpIzoEn>*8lE$wF$3-M(re@CBQUx=?)`8)Yc zyf8J+GVvjT$5$pkRPZ>=#D@v)uS~p1@HoiCiv{<8CVrLRah8b>PmRY+9HU+3GZP;v zoLTWvVsBP_wCKr-+f(~IGW|OQ-|v|?{*&NLM@+tuKH9!68$M1f%!-ds?Y~I3Dijk0 zKQ}V*t3|ePT8O)uzZ=ff$KC2qM@+s@OcFdU(`yUGWU(%*K0nm|8zeq>TrCD(fZb5m`?MVRSoDi^|HE3~&Q#O` zOiK=Yhx9X`$K20y;E(0Nf1LyW6L2RxO{1^m(0?0vw*4FLN_~53GYIeBIGe*yA_qRA zAbUJBfTOntm&T|OGvZB6u0}DiEkrXyS|e=?i3v>$M6?Ypk@fx-Er9N}QtsVlw&>*8aU;Xay8TD)@xf*t z;fiez8UFR4M%1Lu%!MO{?%EKJyIP}>Ry}HL7OoVuJ|1idlm-JrDm43J&B7Jf9ELNK z8d0-`xAsUiC1}cuMW{yg7C+f5uC>JwF4+?Hw&(T1YG5>%XM3JcGUas)RAf=^SQK}IqcsED^SqO(Q)zPitjDw&^|U5o zndhfW7b<&-pL95W1&d8!Xbr>mJdaILrD(l=w%-nhVq<%r?=sz^6ghrwpYzwH?3XG% zUZ*kbNwX&#tNmlZD5q-qHSoKv=D*e%9mk>EibpK=JkMrI@ue5#Hudx0jmq@)c|OjR z_1Oo@On+ms?@<*@+u2Z2wqyFN#Xhx-vS&SZW0gn4m@tli zo!iu0m&2agN-yJ7`$|1k#r8L*5R+m1bjo8VUA0R&EK?7bsi>TTgX5=X5f%D>5yvh8 eb?TXKGNl&h+%D_VZJN#g!-b~CH5LPlDEzwbNdvlKOJ>RNrs4H_g1Q)0Hx**P1X(B!rtmO)1Vzroo^Fncz z)SWURnp8VH&4ic=x5-nCa;hrcPR~f85KLK4b~2tu#weORTXdKdP?+Z0O(lGZ6pP{Y z`Lw^xD>XQzvl9aEVLVB6m^ zLjOs;t;%jxHN;Mrt9VQ~-mS2sy!@+$FY`7Puh@0)s(v$7ms94Ge9*Aiw0Q@meSq5H zR1Ki|<%%cy_;tV6$F@Yizw7*Gw_N?fFCTyOySIIfR7w9*9OReU@AjeAqxGOFaiYTy zUbpI}&iiMq*!R%Si7!;@0BQvU)W$-10{H9#`oSXjT}AL;6v5Gpf^iQN(Qhw;A1Q+0 z09?aiFNXmX>eqn8UEKX) zrm)dylFcCXshUKZyiv6}5$o&-cacGDGTE4Fs}K3(p{6dw)Zdx}txkr6M%XlJ3>wL3 zm%eJ5SEQox)jkV2UQ$zj6YW*h9O`a7Ki9?N3WrAh6H_C)V{hi-2D({OcApEs*p62>YO1ioE(Q&W6B=kDUR>P~gl(<8nCItsGQjM zTJ-01!=c!S#^sKUm|E_PtFrMJkjmKc zu}V|#o*ccj((^*^o@YoeJuo)@5BP(V^Z>j{)Ezrc0ggpnRWC>UuBsta{!A+C51~5< zwex9$Ux!j`B;xwUwGo0{JI??}4@YWURd0~?oP-I#7ID2RBS^i#>1w%P$ke)az8d{HOpcx-*Czot zvjxT3Lky^S3%hoWP(MyXzj9s1wQC4L%b&n91R9;DW^WHs6!g0J1ya1bD&uNCO?IDA zh(Cb)oci)AT6hi)-lFJ8<<3DeISWzqASu31_#Y{G=jrq*Nk>UAM1nzb3ZoV45FS^j zl`c_h7ks`+9!4Pg6OK11(raW2gUV_jPQCY2>s;>gPq6-j%F{Sb;CKNZPr>!BXUXPy z)SBfLKiIGIpM`$lbT9paK?No!alC}%X)-%WW}|R2LZ;_%s(kba^zdx--tOP!-|heA z?!o^0-p{9{r`Pvhbh!3kgMkDd_l;B5E>3gppM~1^2cKK_8*YgoVf}|&;#VNJCGP$V z3Xed+yw4$zn(~vT{C1M>v*dKYo}RX#tW2E7Q_?$a5G^81KEti2Ho#ITWuTOSQU*#H zC}p6Ofl>xa87O6-KnAwN!&)@nnKr6Yi#2{rrB&S-OfFsQ5lxAN77NC^wM5#0juuR| zrDNf^k@AQ}e^Y~h(E*cJ|U+!k&%63GH>2Go-eU)b>QLsVX2`jp9!Z*VMduA_I?^b>>c-E5W? zNyDIBprfF)7|`C!W{(p7K{h)AN`>AfQ*qq2O*p#Tjs-KPRrKR`jzz%eUE0_?rmdLe zuA4RYX4j0p6+L3Z{I%CFTXYTCkv*2}Vk=^}2~dUbT{r^3dlW|G1{?;C&I{SBeD_&9 z%YC4%`qHU24>%7^srz<$?^Ndv&gp2yM){hHd_IQb!u#1Qzgq=cDy0mRGEmAuDFdYp zlrm7tKq&*I43skPf0P0K9*@7j<8SL;RXJe(oQ;25@p$`U7PV->8!TB)sqZ-FDW1Or zT%h<0MdvHZ-viQGi3-az1d(N~YB+!6H&@|G z4U0PyP;SM5lJh!_UNchpvPr~<3dFcq@!U>Y4^rW8_&6S3#K`uKsD|V9j^sHm-Z@I% zt^DCltK>QFSl&tgxU&C5@m4>~u>Z5O^&Pt{qE}sipQ2k8-J@uyqCJZCD|$rHW}oSB zQ+4%Pt!hV0I&P%3HJ)W2?~A6`Z zCg&61JxILM_9G3Yh4lG34OW&_I6HFML-DB8;cyhlPYgKu;paZ-Qt8Iwz^_pJoR%O? zhjA2Y|KmmQV*UL!^c~_d@jP|JRtoulPU_5?l;<cj?))>hHl z*{Rb$j?SpAc7J#P!A0FRe`Bp)ySavTwqSRN5fm~iH7J$P+k^2C?Q03gLqJ+mDRUc) z&}(*X_BYm71H@h@y*8j61!}ed!k!p*xv62xMt_68rLJy!ZIj;Q-`G%#K-AbT*qEZ{ zZiG`fYyf?Cqij?j{5NJxeDV5(hsLfvsV zGpUg@CA^_WswqZSR?I>*8SV&@y{dI~7{Vj3ga=hmTLLUqQsGwNF~Znlj)K$Md1vt$uk;F`Z^d6_cjsI^#^dFMu(6~Ki z;rSU;P2o&p>VCuEm+2h{=Ijt$!p67*3 zd+?m1Leq7svOTW{wgaQNJKOX8ktwfRpdyQM$E>&=HCiLEJn}uVAqk63bl>xd49|Es8Zzib9~NUzp~$` z^mtvybRfr`Z0z&oIDNK#@ atbE1hoa3@C-KK@?&#g8!uCN){Sn*Fh$$Sw2 literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper_ubuntu/J_RightToe.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/J_RightToe.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..963f5aaae64815ea2adbd5214d83999511b9fe71 GIT binary patch literal 16832 zcmeHOYiu0V6~60@^G?wY`#e*X+&` z8$@yB6yc@;^8*Qxkb*?pw3VvBkBYV`IC0{ng(4>^#ZX9{gcQ3i5SB-2pxJ)s&OPJV z@h)nss(-rInmOlu=W*|xxp#c;&fJG;>T63}F2Thuz9C5Rmz#*66>FJ+PE?8+I4>60 zO4~`}stMe|DJH~Jl$kQsQA(_mo%EDc3c-~18&Q5E%8#jFdqt<3VPqSrore_eq?)p0>hwFQ{G4=&@?$D<0c`69 zBmAG#w^{j(sD?P{N>v|IuJ11RQC|Ml!nb*gs;|&}@Tu!&s&1#uC&i#{v2oKjN_#)G z#jP4Z{9GlIeoy6TZ93j;L=$ll z>)aSI8cebYq%KvRNK-Uos}iw}_DCl=)FhJ)sn)u1ARcb)G)()=NzkffBxFQPr-qP` zjCSg4R`^6J8gEXd$g?Hdj`rT0jwU1FP)GE>NHRqgw?}u!I=!icWFi&^#JgJDu5XF9 zwi%6yh!+t;t5=GKnkKQazHUR6ev9`O@2c_45y#0g(GrXfHH?Hy{Y_f%#B>@TSL67b zX$Fmeol6|L%N#od)BT}Be(!_wxJ@sNPNRBeDLp?w`MGsb-QZ#Md}REv!khdQRS3pe z9$9bVG!|8835?osz7|qkwBhPsi?sjThR=|Qd7AGxDcRbyq}gzOrjccZ4Ojny2o^pY zPHp8<={Grw&(}tX-%n>D2uI?q67-ua;nOWth|M;9rVTH$#S9cPP|QFv1H}v!Gf>RH ze>(#od#=3{?D^}|;QrFHHwzK`PQOu-9SZgwn>rw?%C0&GIFr5pJcwtpAp17bWX8^B zv)Mi=)5I+^cFK}z!j>63V#ze|%8Wg4$u!~0j6G|~G||e8J#NV~!OD#7wPczsWybDh z8Lhcqwx(Lh_CRiM$TbdmokL#bke4}RXZ&j%`gtq|53E}NK6s#X4v||wH|8Ogk9y5i zPRovX7I&ldVn9*YkCF1gs&hn^UJmwN3?BJleelTV?x1Thczu5VC70d~^KA_CTrH zL-Dr3J#cqmTV3y`fyS-D-dE5ed_9+3p6|qQ=X$a(&to-Ug8Oe9eFm{X>w1rko@RD% zbPw2l{YDvC?3cV}-+6QbT;*S|*v2)_n=Qoh{?SKGpZWvoq0tw>FF*Yf-P@O`gHTM; z*Y(!mdQQ|Ihz(0Q6Y!K(FPUH8`&oVOhr!-~(OY4Z@su?TnNX(MQx+RGt;#Z<@`l4e z%408%mYa4D+U(NGPJ`WpC&@0|KRWgg#6ysDKcY&sEcPN*aJa=&@kUF)Q!xnUJu+E$ z0GETRwjU?>9T>%iTRh*qzJ*}V_EP}TLoGF)iZf(AFJZ!Ow|G93HAuVQi7L5J$kceY zztd9fsdx>DTxP*%`|A+qBO4U%EO^c2cMPEkM<7>987&xuuvrT5ik*aH4$Ep!(;F1% zHx&9P;7;~<4RHBw!4o~*grHPLTc!3iSs_ zUs9XjLQBrW?gG_X>gQOUspm18Xm1^;nH~Z)m zM9gy(VHm1E;W$Gzy-lugD6jI^De0RsfL0MEpW)V1TQ#*viWw+opqPPT z28tOdW}ujXVg`yCaAe@lctnfFJJLo)YN^I=skDmwLdoSzy`nLZ&|;x@mzGEyu+c)v z)^scqH&R~F5NNCqY*^c{wJp>U39pTXI@{Y)g8Z~-O52l6#9L`)>M)DO@53v&@TPRE zjw($})C1j=Ft$WO?RQ6-jYQJXUL|(t!WY*+{S=iqnZ9hwW9wau+_%xYYx;@7|6w*u zOQs=ES{03e(p+ErD4RV*_$S%yFenvzS4PEk{}$ouEORZIIb~`ee&<*MjNYM*eqj2F z*=4n}=ily`v1e+xSif-X4J(#hM}FjwWxLpnI?4!8h439Xg21~KM&u?O29A!4*{po` zSu?xr;gYIZldB(fADC48z0#h^?wj1x(TWZ7^%li^4#&lhvsr$(3b9y<87O9;n1Nyj ziWw+opqPPT28tOdX5jxQ1N+zDVzgQTQ9XgOf~|zpc~gl^_aUg2=K!HJrcg zo3C(XhQ*x;sIa16>3JPTuNkRaWfC!>Dq_4+$=ps_4^rW8_qaa1h>`6dQ4PoI9Vv6Y zc;_hP`6?dXv`U%tj^&+{f1&*Uu4L;v%xeEQT^Yqg4PyVG$at=;Nf;q@(Bz9ucj<=?)==k={zYQm2C^NDVJA{_>&C-}LMi(e`7^;0)SGk-svYmd?Dw<9L+77GOTOS{&Mr`E2#_WX1kx2ADCDbM%o z^xP~F6Z47h9wgr3_>qRvLiYTehA7J#oL#o|P<>Qtak%p2CkCA2@N=JRsdV9R;a4br zPD>D{CvoI!{|^e_g|7Ekuy=_o#H(~6j*>6_E7E4cggl=HUV={U>~F)sU3u=;1sP|d zX!A?PI{jJU&inWoaF>|af3nhkV!xe(3#NLVajup4#D29BcnNkxImhW*;B)ByI`%&_ z0@q~Sxu@m=_(L*It}RL5Er35>0Dqv?gY!La{b2acedDJo$L}V$xtk!htsiG7hG(T4of4))V6`6 zxmh%Kbm+8?qa&)T?H^u1h*7&G&`_h-Y^tWME!ZAngoLb?T9iuYZJ~IWHnv3KVIaFx zDRUo;(5ts^3N+MJ0mNn}y(Xvv1*^9J!loDwxv~Dv4S{<7&f40oHH~^>U_*TkDx%hY z#m*Excel(}ZMn(WFQYcnjBlGU_rUo5_I)dk{XEEuV`mL*bOD;XL5Ftc6xe7Zo0oUj zkKEdWJvzdh>WUem-JnL&q;1T_6Gp__8c%yWl8KH;(&!Ri3)-EIwuhHR!$KOgg;H(8 z8}5oDm`RPKso@ z>xAqV-bkD3a&2KmW66wR_C3>sc~ljOMG*|4DKZ*O{mYAPB)k{^^dcwUi2AoM`VUES zXbewTcz(uIQ#cczU$ac_i0uA6k7G*jTkJ(G6D%kI?la7xxqe<(F!d=*_H)*M2lRM) zTkYq0A=7R=r>M|$omlqAe*#&y;!JaQuAk?ROnKd6TAMMLCwdPKS|hMO&oh~7WQ4-) zXFH~C(9s%%WuA{Rt(4wo`^kpeuVAqU46Rw%pXaSiHD&0$e)fMD47H8@d49|EkTT@< zbN!sZKIOkb+3~uJX}`^%e4PGI1Ec$*mRsZhht+)7IfLVNsIcM*hdCeHc?p`iS+|A=NYp}c^*G;y-aByZ1)#_W$QC(lME%{#|}kv#WqV)nzYNsQQ(;rDFdZEW~8k-%h#j*-iUYK%aWBTm$6_9DF_WETTfc i4{_Tipicd(O=gKBIM>UzG)(jPzguZqT&jP~Cg0%dIj18uX!Z`e|7-WY1Vb<;+Ko0_Oi6Pj8>+r(g-G-y&?0t$r|5v_%4?23@=;NP+aR6wTRx$mCYH*aT5 z+cZtn%$>Y<&iT&Iz3;sH-n)0+yAM{^)s)z5f{R_;E=cm08Hkq^OPPUARETld&lJ~7 z+Yv*mVchP~2Ek zm-dtx@>7bkABp7Gr~LYqA5+2hik2E-WE-iSdlYV^E@j8m>UUWAS?Mh0$5iG5*xIvt z_}{5-welNK4YAUNsy?P%-(B#dy!^9;PxBg8U!n8hQOC_xolcogia~v5<*G(XdoQ)c zt{Oo6wMr)a<|TdMBhU4n+cU1kapxbe%=zGkyJKWZ_T#ZpTx!453$D}E30C4nk3atH zCC}R*9=GU+Pye3ee5D4UE02QOm=BKwpP0u!Q~((TMOX33*d8r zyRccyQvmWESFgk!!uw6bL)}iq7&Np@oX;5O(7sXGbFiiI<^V7l+!T$+f=NA;(1Sq{ ztZk?dhP8yYDU#H+L_>XLTRf&Ugf_NmhAzJ**xV5cwnSo~w#faOh-w`*saUffiN{2= zW2L6o8)OwoZL%t!qG-fc#-r_RS_e5)Cld9^O|@ZPEZoqc8}_Rcpp^+Nq-%y#eMnD4 zI)Y0UctkQ1YmO($vnA4o_THC@B(!j-J+fI#B&p)I$i`@gI~kXZmgRuB7mM&6!8)x) zZ-{GdLH0cJ~zyu z5wNp~J$D(`4#9MOsF2^spnTn?rbWh3J>!+0@1K0%8c-+rq`E&czE|M^FGUrCahCg* z8#rByDzpUpO*kJ5DF#fqdPGS3FHHD2i5R;nUW1aYIZG}R&i6F3EH~lY1qt_%#S9cP@So1W zXO8PH`MW+U_3s?{{w+fIAL`XhGROU02TKpgsxpgC10KxW_yLGxrXc%P(hR2G&tx(^ zQl^31VET=$Oar#T^!}_&1FymK3t5>4T!ZPKXJr~_4W^&S$~3?lO#d(|(_m>ZeILtc z&5g1(RYJB0a=k^aw#dsY@*;~o*CJcv&#>qxv+UosY$|yFu8|Xo-1>z+8L52CZKQHc zrq4066Rj5q6oq}7l)Dz4CNle~zk9&H|C8nZ{a@Jqw%7b`T-C2dfb}XsX{N8mF%!qh z{*>>|Wxs@4q;78XcP*PntiSuLevNU!l&U=x zuMzIPdwh4-c3<>0`Wn{yyN{wvRCHY`aXfSd=c+4Xb3Fbg82`@O`hSY(ptaox`;RmG zYJUps_Fmmd7TuC}Y`>2@b}d>ofylCVFzcSz+yAKad7!j^tzsz9kFxx${WnW*z0AK8 zMz>4BvHc=?5JG*47zov4gz`zR+v;|0>+RP}5t_iyJMl7gnpddjQ4NcL)pn2acORK6+SMm4h#9+)exFp8glfFxO5q z>^y@o+e>EJzWyI`yxnBHx5e@4MGC3yqkVKwi_cMhq{UHwnwWDiS>KDgn$D7V5aPDO zEslG}&%mLQ{rH8%IGWCpF!B~d*xyp+DE~XqBS4#8C-=V$Ry&&B8g#5XNO8^*bOy@6 zG1{F#b$^BKBhU}v)<^0S6yyR~e@x;>B=!wD0)Ivu=01<92pm3y@!Ve%k9tm##g|l% z+#P}KCkQw|HM|SM3#2*;CXIN3&jCA^#mE;@nsirNsvYIW5apinw;M47?;*fva5+yE zyiWR0!If>>2YcuDXE_P}6cxQq{+B7z`^2rARZeX_PCIHdrE&6D+2*D<$>%g;9)Z@` zfEbh=7`DNw+7Y1K1kO!O?Vx0AxHcK#^z{ zI11@J7-!uqkN*($H{m~2DW`&dUby(B z3a5WA>W92q%BcDXgn;sEdK1D158MNJyyE=)YpbbZ59UVBrb*QB5HxCnxEFK0P zNK+3uCaoMz>nZQ@WZ|fsWYcEk=uhYj`{~fbNFu!6@gM4FS!0dPi@zg(W$YuU0>N8 z+Z|VM5qDj&IUX9gOx#za9osL1OTUkux}TZteR>jd)!r;F+ay$WeW&y zP#=B?gGn$j?z_;RDwX<6AkZHr{b!e0PxsG5qoWZgyC|#k% zrc_jm=}EWMIU4F{YfTDjgDaABZArvqo7^PMH;G2Gc;Cs4K?jyU^E8!HOkXkN^m5yD z`)%|toBnd}e45G7h`JZF1N1m3jaHrKGnqZ0t1o6UCqb#uyJaf2`_~9thtoFwn$e{_ z_}^m|FnXtU@)N^XOmNmrm~w|>+?LW#v3%Onn-7 zkvZ6OY#RnL8Tl@>dV+IDN#*!aRgc>r8-c$Y9v#&++WyFhv1rE%Ob#gKv)I}%WHS7& z6=JazGf>PxF$2X66f;oFKrsWw3=}g^%)tL~2KYNX{_c*ysr#EcW|lv&$R8^iFHel3 zCMkH+Buj((j&rJ#`TM`=O2(h#vP@HyzXPPX5f#>7xSENRJV5V=QTQ9X-FR6@g}<$H z$u}uO-^fi>4aZbRmMMxVGfb=uK!p{(O3(8)dK*YZ9fs&r6)|3+WNs(T|ETb{dt4u0 zLCE&^sfObvpOm>?yjYa-6crC|TBXc+$8=80FDZY#O^|Z-IE-rlYy0eX?3Nlm>il~Z zU9IQ_McWnaRJ2FY-HHahM!=Pol}laajT=)jJ>|O9y}<36J8wx!it~Oj-{bZyoNd6C z`tyl)`c{<_Z8EUqD^~ta-UQ=&PkXkX<>Dg*_pe-hq~Ly-i;og~zH;%=g8M-(UMl$f z=i*}o_p@AlY_>n<;%Z`V2Ep6KHNvIxY*yjxT(LJVK0);6#qoW1o<9rh!YTNA&y5db zml-j5J3iVT$cs-B*Xh1lB8KM^A3sRE-I8ZY3)%C18lo&qu(z4oL-kRq z!Dh>opD1vO!}oo%rLq;94gWgveOiLpJ%ue_`=2X-7dqZQz}_Y%iFfHhEG1w3qta&T zusojtUXrK3odj;nbH2{XIMYO{S2Ee%IfYx#<3->$F}(j|r2X)II}ry=^;+XxFY)31 zY9a6vtcJoN&R&;CPWP^0ntW;Q7{>h~|Wz)Ke`j?q)HxEF`E$gUz%s zB#HGS;dpRUTYO`vEf~iC(8*vZ)ghYY!VX;vyDL14J^77jy+|Y&N+d#CgZQ61u~oDr zLQyRkPDP_z;bM}4u+(!*O$#`hn?-Ybdyv*~v`2z!`G*@2V$`hh)mI0rS5?u{7Az0Z zLqb+dElS3Nt)W<$7Pe@yFp!POq_GZ01gn}>`RZ#c0b;RJu-dNz`K#6d!lD=sxw7ug z6~4OQoi#OUs~dt1z7=)VsEAtoH7iqsxvOQqZplr~dKtBlW@y=ru?EKLHLqK-tmi>i zEGuhhp$pL51v<1cr@%rR*}S}~e&o^~tkDtf+1I_bcY2rU_Zd z2M(`k;#@z^E0}teCHqI&GOdu_M*GQz+pl1;1q{tu*q_I( zOkK**di?Ca0}Qo|{ds)LbdNIR_H+H5zaHhkLfP@WjA^gQpM0$T&j6$IqNZCz&tdgE zwvOPq9V)DN(&Fz@eoU!-=AzWu$Ng?7&Gqy6nRJOzHT{h5HXm8*cvwuc6scz@O?emr1IAWu8^B|4muMVA$VGx$l`x zdsIM=x_)Lrxd0m<58aEX(Emf+b_u9m?_z_QWeLvpvMtRE^7)^sFf6XKI9NpS4?ZU^ ARR910 literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper_ubuntu/dJ_RightToe.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/dJ_RightToe.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..0de7ba77e5da5c88e74061ab6f4982641d0fbbf9 GIT binary patch literal 16840 zcmeHOdu&_P89$Eu2z^}_=++0N9&5D}sBs@@Ls6;SJnFEdls2KxXy>}IlX!~lU|+Wh zj}er%5S2oPGzpkAbrT!iKU2pa6KoQsYH2$_Ba~MQjHT@gNudxj0WE!azwg}hIoCHf zn$R>&lTYOPec$hUoO8Z=?#(&(-t$0ZO;wT2Cb-zet%4-aR0HuSMe%aQfOtfin26&X zajmo+Gol*B?HO-COohXcsgAM4D%nc!P<~8VPkypKCpn`y4>1z6DY( z`c`Dqo+3kjLQ(c3k^K6VU%&EWD%f7pVk3-fBeip{!mZS)?3h~pjwnAXovZwq%3J_j ze_9WHoBGx&zaiBSD_x@MW6Jg20YA#izgze;uT%BqI}dKvZ>H*W%6w7`8Z)cc)Kl7f zsV#QZ0OGG!GU-?TdBc6n9<5#QlebU*_R=#OPP-l`KT4)#KN&xYOYL`fz;!x1!AhKH z-(x>m_Ivxo6H6a_@{c6vDpdfTITX~!TzDM#l{xH#dGLGk;Lqp5(Tkk*?#yG~k_X?D z2fq=x6F+Nt0zj^Q^-5e5?(Z5N>UJU~prKR6=V=2SIo2zC4tBk~IRFd z-6ovc%B9R>a1@{WMu^8lM`yucPRP{=?b1BDC}GEm4s zAp`&E4E$5O_G@3)C&j*9V^7>9gl|W$UXC=$P zM_ooLuSoZ6b2`y_aX?Yn50Y|s=}98DzzK?z0{8y?$B08UZ$`0LAJ4 zCT$M-$^Mk@&K19bTBL5M_jRq9NvyB?v_8qVYsF3o1II6?(*q$S>(JQyA=@^=H_Lx; z8kl+NM2r0E}ENf<@1bLK@4)k9 zm~9s}?3}|e+ec>M=LR0=&uNL z8D8hf;Q}2t&Ye!=1gd(6YWNEUsF490-UjbK4b4foy@jMrJBzg+S@lt{SBVS3U-Q39 ztZcyhTI+P@@N)Yo&$eSh0xO0nm>= z(kY@AG@JzU1vT{@oSiozd!|SneBK}{v0_69Z`XzHo%x;Q>Hp}OmmP1`=_A}-pli#BbCkldy5+V+Xy z20y=$xqq4Mp9XB$?~okX_kbJRI%1EzND}LD+(jM&!42ucc^G^j2F86B`azsX=qDKZ z_egKg=;?lWc>JuQB5@f1rQG8WpjU*+|Dl=xnyRY>Qpi9d1BDC}GEm4sAp?aB{10ZJ zZd=?LiM6HllH`0Rzol}PYz-zB-iX7Z`Lw)5srA;?cvmg2UEdOH3x}3RVq5VlEwm;T zttRK>Xg$z1aeZAl*m`HUQI97?T|DlL24fx0cuI%T8B8>%qT!gHbXlFF!S>dcq@Xr9 zBT47BL_F5)B5{#PG@8Z7PG$@`wDPGZsT^neq9G5iw9T^LLhrKak%RlQbeg8wy`VIs z?E|H`tK;)@dM~Kw%XIn}C>46QOvQH3I$>*f*k(-{U)+P|9&>@wJGEn<8opwRqiV|Z z+q8+>iaW*1nai(VJoh^CBmWBgJgCD#fGUJ<#IFo^r^1Nbh@Xz%=Am?2z6-6K;@DYK zK6zZlWA;bK;BmtvO0OEO6KqXW+@rZ z%w?IWD1Qe?Ya=SG|Kf5wPVx}FBSztG?DpVgAr=0%&MDujjC>@LO z-n2@Y^N!`5l>ems@peJVnSL16{@0F~@7OIhy4CsjD7seB&5E`u+No%dqI(qedyIgq z%gdKLOX|0zVtUGXvum--yzyO=39=d|a$+lVz?$76DQ zzfSkfA~8Cj`1nEMZI*{Jloqn*`!qyZmf>hKwTJ4XQiY!_M}DHfDGuNF$(Bk7el|RH z;`_7&ad-m1THH3x8*orAIdm0 zMT7;_K;@bW+S`KtA$m~AdZ|gtc%UU13(?M&a4ZC5OEPI} zgb{&?hBe;W>T-bC?G&i=sX)Gpb%3xdhC{BdxqX$lCUARI)%wc1K%IA0O(iO#=6=K0 zltA`&nQz*2leJ++?W7smH)Cvq@p#PpRxBHOkQK|;8rtXrG<$~*ZOzHE(?&Kg=e8fY zw+EYage%z*)q`6=^@KrNn2W{ru&X(iaxAaIeB?Ocgi!e=uTZ81UxVBbZxa65|fps;?#wH^djX+q( z<+yN#Thy3q2_YIwMhs){86M1|s$evNU z+RyVtrgR^sLeq9)*`L=1>w(c6p6lm%BvW3$z(f`mjzw`7I9e;PKhHOrI+Z22pY52o zKu>ECmU&*vv`qO^`^kpeuVAqa46R+*pXaYkoyySKfA-%AhT6vdJkMpiR~d5qxqi-H zkMduo?09{~wAbWMK34yyfKg7>vTNl3vHHJkoyl=KR9NwIi$Bk=nNs`AMTO1$+usAF zxqhC9Gi7^@!7|g|S^RsIf@vo^D$0ILpSAdB_HV0sr!A8hBLMqx*yk+%JpX6P&jHxp znm=PcICB1lQ&}oH#Eyzu>pujX^2g(c*Aq;aSp2Q^ziIL3bq7=GpSke(VeA7iG*{>n%9@n<9cY%zhm#QG<}Uuonc5-5HnmI4=ImSBr}{p) z+aoq=sgXlklLpKmtpaMLR01jeqcl`in@SZp7!uk<$O)ht{-Bot>bWftjuId$zP>j* zZ@stPqaRZHPj{@{H}Cg8X7=sudggZCcN$xoN<1D$#mm0Rkn=A!5I+gFQ3YgdD_aET zHSBt>TRJD2S36Q}Ktx%k!DAa`SQWgTo)k`rk{s>0NJe8QGC!Mi3KgIbwc0H}d>t3F zsk%ZsUSjY^1SLCU(JmwGGQy51BYih>? zZ9V*-w6|N>Wkg5pbdzY0D7AM7*kN4$)k3#K{Q{L|_s(s+GUm@>QngEm?1lUI>JP!CZ4*H=Y_6&5G z8q$-IA$7|}A4^4Iz3~)U_C*GvzxSjgNi7^oMDEp+DQtK!(i0t0QgL3<%o-rdW|mM} zv_8E(t|{Okw0RS2ZR}(_Tbg&&s~eOJO6^?WphnMt0hYkCQNldp@2%R4{JFRmgdYhj z94n}C><;6ZK^X4~7PP|$g`U@RRpcscVX=_Ydz0Q*m&A>o7VkyEj|;rhk6yvQK^XCw z?FN9)pm5EBtOchbfPzaFT>MMn`X5{HMI13sEByvVU2A+~3r_Dc)U2`K{QnYyP<(z) z82Z7!QrYS^YKYUdA>#MrnQ?sxoLPc?ql)?qO_Z_S7JRVR<# zf&W$nK9H`z5*+<&MetDBg_{`*J~*M5lClIC0C(oHTiyfaPYMHhG+Qt1@++JvTKms^0B@GLiw;_ zgz~C>+L&=F{F9{XEcaO?~W2;b-xkM!W(a3@Fe9_f)S9euunv0?*wq_64Mx zi+xhfb8uV%mF}rrK|R zIt|B5D7+vvI?sa1pMdGgVB^n${8g}l?shI}KHX5wS1YIsm)!X}RF*ET2Awy6gRXSe zS0L0-T?y|1-(7*df$qTF-IFi~55WClta0|0Dvp$22(-OQiLh)LNOo zQpq*oDy%&om!qNBupCe8pd*Kp{pqL{(^Cp-4Yaoec5G{H8we$|@V01ZXmB9K z&`yq|@J_8)k0+h`KT-%^+Wz=s zSY9LgtijK0_pJ8bir=pBCkFp}`TTC6Q$UA+W`K?ZmEX_jj{%+dAfKNGibZ_3WS;x> zFwan>XZ7Opig8${R)Y+`rDfkWY}t~^rX?$HlNRl-7-8G1w%xd~`io=_%XYRK+MsV~ zgztq)5bzNMM8WT?Ivj~h`8@yj*|?0ZUo#2xDjw8;6}iW!2eGKXg?n9zr#;!SY9`Xx!3MZ z8~={r%{^#h(E=Ztc=@GR5UmnC?E_pbc-jM3C3xBoh-)P*B>!+WA4mQYej|oLd+m;v z8a(Z>lksyP6xu7dQd|gM`~bd^OD%i*{01Q0vI!xlbsT;|#`1ZCuxZf{;Xc7rKXE;X zh4%1Kd+>FN_kUV+96nTWp4x>kU!1QL{@_b1=jlGc@{aQ-g#9Oi7nf@^`ae9I``B%5 z`o#VB3%Xm-eS#(g9T9X~(4&HO`VEIW>+844H61IjGp&G9DZq#t%yv(mwDTTr=BAC{oGHXE^=cLGlHMXnmfgLT4&=7X^GZItO!ep7x%D5>=ml1h1b6GP+3XI=x&0Ddh^FUN=i8Yw$ z0%&1=4kqRl$+O|zb4>c-nLUuA!<5u;R1ft4)sqGtpjs@hYf67Ctt681gqGBYnPNgc z>BwMsT_nu7!ayiBz?ATC44fHMPZ|ZGDX)Q#Yn;N2ikEyE);D*jJSa?cq7j6VGKRPg6hyzC>jB0;2J$cr*V8KFpQW2 z;~&4{I6V0OCWQYXaSjcSCoeQVBPt7=2+gmF$8SVddz!})ttLf5#WKM}15mpRb7*Rx z))hp3HamO!UjsR=DafAYg+xc-J%t6Q>sTdwS`V}VhI4nar}-mMTDO1-FTx$M>@KL` z8iDL-o=H>|nsnWyM|1$>xCSAf=A%Tn3VZB7>QMg$%=SYC*DPdD^H!p=P_$n^*&l=o z_KoameoOS2P^A7-`xL)%VZTG@(YlQ2gvB0h?DmfXhA|b(t-1fhV!msi!BIbiTlNE+ zJ?eeP=m;4KN_IqlVY8?CC((Os3rn&i zwwnU>$L|?oPxqh6LQ~L;V2Rk;6t*PkMHH;+KFi>QMjp`3=dwBKFvpwJa0u3w2XO_BWb{Q6YOPrLkvK9TyJiJ$4-k tH^M>JgYP0N`1=s`ode=DvDv76$>yBeC0$$=I@!Ou)zG-sW?*C4KLAc09WG-h+B>#a^*-30 zB{mYp$RS|TfT>g|AR(m^Md1$$ZPn6N1-pR+6jYhehv7%HghsO429l)(ib}TU+ zc05ZTs`{sMt(kMrcOLi7nLFcq_s)5sskOPx<6%_1>^6p6V6i{~B-ldfzCY?Y9C`7Gxa}Zz8#cX0{ zF&!-v{7Fg44q3FzNxPi1Bg#l$(kkJG_YwPfTHyVs}|)ek6~ZD zvIAIOCV7-U8+_!Ut@-;`v**_DIQg5J+aA91^8~7*{z5p=FZRDW0JR}?2&x=MI`R0I zw?6NEWPbgFPyQHrr_v0d>L9>AI^jvcmpJH0O5pdEz<*i-hpTXG_gD%2z7qJU68KjE zSK+XilK?nf*BHkYb_#C_6!|(~bD^V)*|mZ|XU^?VUBVR8*;8^HFiq=?Clgx6h@=fo zV_HjFSc~duy*HLI^mJRep+A|>+ag{4x{x_bwC;h3))Pxa`eXO$EUphUXA|8kc_AL^#L?!N~gn_-j--E5p5eVg#NxX(1x@gF?3-Rju`3K zfVOp$pJif+?qmindt&|2-+QvLv>uJ5V)yFl3^v>!>xvKfGD%+1%^D!SEi9$&kM;H$ zZAsk+9wJ*dvv5-f+tb>zyFuIN+vuyGi5%AGDG+EGJRKB14-q zVW8(>r|%HY4#IeUu%Oiyp!D2kYhrV;orO|P?@xMfU6nU@QobJvKO^yu0D5JNaN={j z1dh+5bj^V&3r^R9f~yu>{%hg-pIGqu91*7#0YOpM8YR_&(>o0{>nu3`|AZhEe}EH) zez31pwgp5Dak@4{0s%ZTt`C7TODG_!s6Wp{8QW*U7g%t&>PEngfExif0&WD{2)GgW zZ%5!mWzDCd;XhY}j#s>U3uB>&#*DJUL}>VxsDsNaW@{Peqp zLSdBiIC0BQziIL~VardCn>V?@c4){Q*8~!P8(lu_0WlmB}i`l#8?iYe9R|8 zIk%8g)(%1M*;z?Je;S1+>MtQ#cRe(6H8lSIuF&`=-jL^9=*{cKGH}o-9aI%^J<3|R zPO`^%5AFOuh*|b4?V;hFHCPReTs9Vjj_*7M%+%Z03x%mD1nXSI{lI%V!M4bL`UnVR zf6!k$md`Ty8a0mdx^na_7!+`oe}cs}T=VipjBOa3dQ8}~o`_Gjp2$v2JrDebi!b5$ zc^L;uXyokFqky%Hl-Gtv&U5{|Qr%G9I`UEL$OkPWuT1R#F+iOY0?Ic+eG+w6d!ee-De{Jf@XW$KB0Km_g9_0hhcD~4UU+Za7>c)GN zy0ajSPrxZWnGY%le+jkv@4%IT=)InxQukKANjZ47NBPE$hX6&Di+FlBuN=IFq6^@z z1DmL5mGXD;%Ar?5T+x7la_ADeffhPG2GxVFg7u-ddm5FxH<0}m;O9a4LpVU^;48rY zj$=Wk<4sfTb%4%+_yQc4q1ldcyo!a_U_AknH?Sk)*sa&$)PZd+Z8_gq%U3PHplI>3 zTBtQtVYRWg8eRkbyMl*;ox!_1;p%W4whVt(nVr`%eBGlQ-H3N;_%j$MOL^^jrE+u) z)TYbfWRCaxpTK$yhQI|NFa(bO9TZ-GunT`ML3TY=PMX0hOh2=>Pakb z6Mb3m)4M#YygP938h>I4d{8Lt13Ce80B8>AC{XoUp>P`L*oTF}Bv34J+mdR<#0XG6}1pa>_K;PlfcXzn8hUFbW%x~=8v+-9X zZ+?3w7cHHEJ`lBaLqY9vqJ0peN-3(5a_y^uuyD((?Op>OO?l?$G} zty6J35DI-Gw^CjRU;F^Rf=eym^aTVU-Lf$$r*#}|A!E5g5SEh-5x!0G)K6RwVxe#M zs6E)7;{DIbj>AS3=c!%T`QrQv=?`{VIZyWimUo>0nY90#c6_2Gq*dCTJp5`E&PQu~T|7T0!rLOlEpzmQz*&BEvw&LXf0@qnNE6x`I zFM~mDA8(U@dmQf9W$vej^#ypvJpEDP_WSq|;2t)6{1mwU>~XsUE*RUj`&q;B+2d+6 z;AM~vWq(e$1CIF&_WXx7z*XLM@u|B6{s8w=tc%>ECGaOo;7^ypUjSU4?VgR5(7y<{ zQ~t(pxxO+hf8$aKKhq^}_|Stn^=CEUOC0*iy3kQH zsrB|JyCVHs6n;<6Xp!sy>*k3ah9328^KbDxD`CD!OpBz`kwFc9^G*-4o^&LxYtd{x zJ_sfjP6JJ&NY#?S(cR6uQz;GeI8rf9rhoVV0w2x$gW)EvX>TK@wm^D_5n;Sp>`^AE z^+ggt~c$E+Wo+59(H1=XRcNIVA4z%_b?PUHCU!7ySzcmQxC z$LWdsw=nzDCr+F(;RVv!ApX`r8 z1^Y(!G`}T!S}IciseOvysI=cL^=Ms2bj)IpHg@}`0K@wtms>Ochvj_NK7*rvNVn{} zHhY>^6UF{ni*#%LyFUd&Yx^|+CQAC`gLtCP+3d%pfankzN=kM_e_*qx`6tnPYzs@W zBeyTw>}eiGl-B>GZ;#(g(w^==Q>CV)Imr^Sw|@=<7(cSlsX~+JX2F_gd;7lziPfIg z8$|K?tcAu8VZVn8&e17MG+(CtWV=4`L~-5;vmVo))-RLm0fa(&f}7z9$N4FaU231! zIh1csfssEKnEoh^&wwpnziBV?Aal0>2u0=RnG>~36z9QKdlrzoenC4VON8u93dUA@ zT3>ZYd(yMp6Tb+IQHT0}I3Og4OW0#u*0NN#FV#&E+23p;qC)moN@LHeIw~Fd<%{J; t5N?8lt_R;mSn&5D>N^MIX>5zA)Y_a=yQGWDLMQt_Y!e!*Z3Z@${R4o_9r^$O literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_Torso.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/p_Torso.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..dfe82f3d51fafa9a8cd1422a5581de4d10964778 GIT binary patch literal 16880 zcmeHOeQX@X6`%9jvGe6KB$SW<*-|4CN|rN;lMv+QF8;{5jh&D<6a;j=_HBIE`aZbZ zOKe&dQ-=?ih8U@>0;;N{@JFgjRa#XQMJja~n$QmkgNhXO(|n0@Rnc&KC`6F!do%B? z@78zpOX^=c*6o}3oA=(#yqR6k?CiU*b4OQ=&nGzf#b*R^{g)^tLMa0#dYvp5 z({1JSNR8r;5@kJPQE!IyW=M~zV0oeq%8YCyw(~gQZmN+SQ@7spr01s1q{meH0l{}T;Hvrhkp5Y6A$xls;}BO1gYOlX*i`n(FV?$?Y(=^?Nitm znkKS*Iq_ItIC1^y)=xeD!guYTeD4qGZEyB|@w!XDFnGOZ49ykaZJ;P zuiNxP|HF%0?|=NK3YmZH0!W(=%V|>yPl4>E739q-_`Ox|pI5=LKN|ep=|KQ0)f=mV z|EvmrvOCp(fEZh*H0UuS>Y)Rdh_{PwJ9|l9BV)TkPA0<`N~DrT z);2S?VF;tUug8d58EYt>wXIBFPy28xY4w={! zQjck8;v>eU^+A!1C&geYi>mwL!_eS+a`B86HPi9^Rwj!z561@*BYHL^GnSJBMBgaV zMqereIZ!t@ZV)}4w~OsNy0^6(H|aO&t>habd~YzVfxCuoFX5xV9>AsASt>yTd)!mGmN z|1HIeU`S<99^1-kb4cY7=e{8l3gK1R0mnHh5>i=|U*w=d>?*+*m*BUT;7dzzFY9^0 z^ML08&jX$ZJP&vt_*WiyCvf$J$oStIB9GL)a-$HD2d38S zTu2pW?{|1y6cuLgad=!%6=qH5p>>zZ)^!NkKH%G2e5;FJ=i;w*@mINcxBaEA@;Vp) zk&fPi_CyY~`JgOvsO|zlk(eRqSdQ$Om@9)Ln>5Au6KJ$zeuZ@8~};@v%wiQkT2@C6=N z38Ow<^aT#TjTJm{Oa3vi3$%OUSNYSNJ)Qp&WDic+O(=1n%m)r0MvX(QFTlbT`uhQp zmZ|*LlurH`pe-*wi*xE1IB_Bqr}Ezhtb3xiIWlo3{{-XflLvMtEiIl*=j=7vwHhBrX>0bInU=M>=u4%vgqa-GQaKxBG&3`lOIS%e ztBam+-;VIMEj@c;X4;BwNth$Uv8+HnEuPg5WKzi?+=RNs;>rDR)E4c{CAzWF?EG>- zdsFsq%N)MV8njax>fPGA4*&AKHLq{=t@7W3XVLh|ApMVGaTn00bH(BaPz~rL;%^s= z$AL~4ip8@)G4VN;@7~?QH`3%=wWPjb684zQP=;q;E5O!KprWyVpQW5aI>+9b~`I~$uGtZ84mpzBfpq1tcM zb&M~ld1(?@QfG|e@pjtt>XC`It`EhAn~_wms3ah8@Ls!c<$T1kOIi8aPRx@2pQAm ziiq=65#zgv=XT=$5fgu-$MtWYMs$+sQKBb^_J@?=?d|Pbv^9GNa!EU<-K?+I zgX>y0BMLHxi&gD(Q-Q)NU;65>b{(LCq=g->!bQtS(+gT;? z`SYp;@EVAUa^I($0KXK*#U1Z(E8v=}yZq8$1^ zri%RU0Iw9UF)if-!X2;irz&<{tAf8<1z%cIxt-SmerbjFi@}VYwR8LS>4Rb}JjAdQ z#vlfWWQAcwQ^wG6YQP*eqVT>sYnZtah|s_Q58I0Bn}ZvJm4z^FByO0Qj5%t+JMqk@ z*q1RAmJ!V*5~H9}!WkfGmx*#5$KaqCOs5SD=Sas53IWjp0s~#U!#$lwXKx3_w?K%9 zZ3T&A+js2T z7T#g(?CRRn*=O{Hx9#YJim<646P;p|hs=Cj+)Y`~3`Nt-#m+zwOejrBjGG$x(P%@{dGpV$d zu}6jOKm)n>aCBWfDx^Tn%*KQs9ZiBUMeU3#;TRt&MvOsP%z|jf8a7d%a_M1P=yF_i z$m&BWU@?=m28C{05O$>FTmYq$qj9$B zFsXzN_X3{zRJytTa}58J;+h+7SDAPn$5bPn39rYQ$1|l;eO~V|ZDv8Dw5@Q^Fy!8c zH8ne?&!D8ub1SR8;1?r~V3*SAbH5_I>U z^&f-`wvF|99nADN33B_ne)iua>2D)B-p4ULTcVFTZv7_!L!Z+2Z0`Rxt&80&J#Gh? z72k5{^ZJ|VQ5Gc1%WbAuURwVY6);Jt=pZ6t9 zv45q>UH@O85ZCg&U+4EA9M9~QXP)WbKsQjL&-i~qra*CVd8Gj!NaPD&bydUED zbao8AUU2O3_&)`@=s!oF;v;8n1Q3!YuX7i!muV4-O7%sEWP^(K6U&74916;%`u!m# z*-!c`=hkPw5tLDe+t2IsSQUM&t28a6`bpd=V*TqKL}ggNl=9pw6+J=*BlKXo3JTZ5 qkNbnqB1{Y6$8DDYU1m0_%#AMNTrbPw_OX(F;ASOprAxuZihlwsoR@_F literal 0 HcmV?d00001 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_q2_left.mexa64 b/demos/rabbit/rabbit_helper_ubuntu/p_q2_left.mexa64 new file mode 100755 index 0000000000000000000000000000000000000000..eb369c15ee8b5bea2bf3a5aa3c72a3b7a0391f8b GIT binary patch literal 16832 zcmeHOYitzP6~5~Q?C@HXK+7Y9ZR@BeEt};{h$y@G!8)}G0XsDcWLSI0_Nv}pvpXB? zNJ*WVpt=-7epDi9D^V$t$e)x{RZFERF(m~WBGr(jb)u>vuXf!wfovK|qsH6s+_`5w zJM5+psr}Qr*33EQyXTyH=ggh)y*u}Ot)aQG!s8KKyy8oOq|h7_330`GW>6>Ai5WOA z5jRQQDU(&#a7QXlh^eSHWolz8u}XH*lgcSmt|vQL1FV=7o*(JIr8>?8H_n8KaZuk@HY?anGYC#_L-Ol2&9 zZGXpz{U_~hRd!>lBTl+Xwa1j(y9;&{mw&bJZQiEZD|a7)>bjY#+bQEoK4{Er-qJ>4 zAECZ@RR@Tlt7NKw>Ve8jxz~U2+Sk9c_>PT`KA6>hrkW`2f<35 z=;$M#U;DK88#7iv^w^I{E>#)<`b$($A4}mW;MbSXkCwsjFN6QI434WP+3x-_`aNav zqh;_f0{7!^mLmX4UDt@jec~wH6cqJ35mo5u9Pxg^L?_Q}V6PWG%h}^<90*P8PNb4r z#)zg3O%qyEYl{}s(|UJ2W9aGDmb%_lQg4lR^y+3^=^CxGKdN=blhNMz{kll#{f*gV zrx8yjMWTPRZnT(W3rJI@K9wbJ#MY$}eZ6`=88oEREt&47SU4GL?Ke#Qt!dD@v>r8d z)2JnCq~rbC+Lb|(i6=W#8M5q(_oBahvhlPYi}uCu)6*GhxHsOB=nrI4lF=;=NMMcV z)Ap>;di5?N00+@Et3*pfyV%^^w5d*85m*seJsCJ~{0x|21)hxx;Zc82Y}_%6Zo!Q> z{%R_qMo5igcbU%&!E|4!klnkW{Jdrt#;>CmW~zFAZ}R)b=PLafP>s$SeGt zM6i5uqY2P6s9Z}RXTy02P{m~%uKuM+{i`;7hD6NM{E$gW*B&3g4d-_mY1Z0s`Jx~Y zB^Z)KkRR$RmvteNBb={|kWh%uLh2)MR*8g6mh`7vs1RFi_)Ht_vTg+22)Ge&Bj85B zjer}0|5gM(@ZI!LWa!UTkpolTxK)VA!y`sTVKg%IeAOx0WMTE2fb)f$-vaS15v1Qk zHTm&33WdU9DboZlKYrelY2ub2KWWJ{0n3j+W63n}%8!5Fl4-(~AAi)6X`+=Mf5?() z(v%z+95A<$WH%@9rg2AjvU;u0DR=&)a!}dcGZ}N zP#y}Hp}eke(YIs}y%(nxh5k5I9$ftI0BHyI@;n zKfV{0sz2nf4d&sy!)@W#rs2PAj|`u~fCvtKmHJHeO+jHN8Z_uXU zU*s+@dpfrl?7k7Bnluhb-nZ{13<_N3pRm}*HP4$Z#PX5cA=9S$U}CKKV0JY3H00$M zo~7~g91W7l@TuH4fi(?Jsfi4qk@|UGbzODy@RjD_OHISi=Wa(epteyH%Gdj<^)b?^ z&HLuG9fv$eKarbfYSjfNwc18Wch?wcw2jbt*V){=AHy4905~1*@?E+@qJDzTyUuns z_-bG1GLP4w(J+<|`*xh`@_l7q1{Vfo*#V*rz8x=;MZ+kdn|la?9$9SYRP`?d*?E!R zm-4!t`+GQkpZD#2722|a9jDNlb1*xLPlUG=`&mqDnq9IF_eY4Pp^q!G)0>7SJidJ^=uQoNjB#><N3D7qpJyr;RaA&Gcq;@e z>8(VVd}mp2DcLpm8o6wU+CLmhEtsz+>MEmsE`b4z9wXF{ z@+#BkJd3=y(>H7SX(4i{P}mAO2HFpr13e5{`+lKt4D{>=g~Avp74^ANc<$dOJpI+4 zMKdd_4r6^z-;BLggJR>t^|!37`7GOGxh~q#27g0id?%Q# zzy}FXg}$pAIQlOa3i6w0!|dw)6?L0yLK| z6!@De;cjsw;6}iWfExif0&WD{2)Ge&Bj85h|1Sc(ACLFn(Wf;kubIT!YxkBzepkuX z9yGOh!PhHUexw#g3zW?J02e8l_rNVwGVceZH4_!C|8Sy^BKb0XBSzu9c1Nd}GVihT z)8{}G-YYj>U5H!+0iQ2xZF~DdCZODk5mnFYIr@Z5<gG%Op(z=ie@8RS2 z@O4V|e@t~8AF8Cx?c&Rql&h6Ld})<3-v=!7qxBv1wZe_IDT>P+YyuZV&^vJ z4cKeF;s#Ou89T!IbFKUfd#zW@6a3yN#y=%WjZ-h4X5L3!tdFNPWJgTiD;8M$kL|Ty zu~6(Tsn2h>No{(Lr|f`+8MeT_2Z{GNeypLekUqbsS${1UkF7t{9+gHM zo)Ym%04G2E-X~ou12{bRHH+WV62$2Uj#B-9vJ77CdVdCe5AN^tbRmvX%Kr;eXTddb zJ`cPCgWNgZegoW7;(om?{VWtcA<0;$afLhY(E zgZzh^ zYq4x1F#r>rq(RdtQk_iT=QE-7^+b~~%4^Y+F(4h8jF|=_wE7)e!Yxg80FmpYHAIx7Nc}cI$cbT>o15?46mHh; zY;4@#(5khDH#Ik)A?oZWB&KM^$uggmaZ{8oqw;7bvu4Z`m{7=`x8g|WK~x-xHI(QA zv^YP95_8Jr*~spdO!|?TJxI|Jfy_X{h<1P)X_NLamrNOYpgWlj^rcgMdfFHe0SoHL z#(QJS;xQo=dZL*g5r_>W;mo8)+N|M(9;v1TOpgRST zn2g>j0)~zhqX3ORXs6`32bDTIotm->S;~E_B=0SI*9ia6`HOS%l5n;*ba>5?rhKVN2a`Pfr>239gE@~aI{8X zd!A=9^(#%jZq{SkgL+znu*~yOrt6eF^`CUO{|Xj+!O)t8?RnnH)UOns*U$F*!BF4W zp69nrk10j&Kex~EJFM(CDLr17F&(klla151 z55PnnwB-B)bDwQgfR8>nW6`Tf=Kxk_MZ4Ig_1EpQJJi=1a6qRhhbLXD% z?0Az3EGk3=K?wxZ_q`9%m;}KlE;){Z~&^!|fv0x)tAQKzJES#5# zYo+d#3DKn5vFRqnRMeO}wK0{dig(ge$|+Nplbsa#$r!~i&lWvL3MfqNcGG~5tdL?c zvZ<6FtupzC6=geO$!<*9jVU{(g7p=xHr>cRQa_I?+)4dPkEzq{oU(J$T4l#n#sb*R zw~Y9I(%yDuH>x_~q-#`rOu4<=U`KKJR}0_f9jd)b_aUgRo2j~;GM?mv#?01jZ4~xl z>Wf!(fa(hrPx8xMkN@WS@15+quR8Mf^+y^%yy=N=-9f6PKL-c-rT*81P#f?Mpek{q zBM*LV{gdANX05&VkslIYt~3Jlml04O%i(F@^ULVRD&Tilz<*o;$5oVV_fQ4>?h5#^ z3iua*`*AqS!vM-%*RaHW;uzf&6!kh0Gtkj_;{BqDPMq6NoiBWrvq#l95SrGNOsBN0 z5z82wCbXv37A>x4^sYqK&@-(q^*!m7-WuE2qnonw60M^zrgbJ#v7W@;x=8AMjk#2Z zkw~XRvTv(yw3uWYNK>{Uog;5ltxqR=d-Ogsh-5M?*{-H|I2CX0Gfe&M8PNKS9y4^) zs3m4(5`EhG)j^R>q&m`Bvg}OspuhWbiHshP^(OApGg)f5C$TTt7s#e%MYn1|0_#Mt zwttnDNpy7^0eFb5TO(Q`?P6Wd2`j>6^EQy$>MIn=tu02YA8_w@E(yX)L^8XWo zD8Y~o*D!{ra#`RUC;9K3jcSHdS2v2H-;Rx;H_5%LM6n zlcX^Ida+nMDtVf?6~hr=)E|tDD=lkcx3GxM3!HP4qc3%dUtd5)Manfb0+%I6=MM$>`@M? zi({R>Ww=hZr+5!+dIDmR`+Qq;aMKd1Mu*-qW=9WiIs_*F>Xl+KA4jmxO#Lc&&mP#8 z*pDB8Q1yrWwZS}md$=v!+BEc+ozbE57!bk1k37Dw--$apSoHWFT#HKd@Xh(h;2X4Q z=(+p_uAa#sK=t6TQ9~MsW!-o1Wef^j<)5+G#x*aTE5yp-`~#*<^O59e^O4+0{z>pF zFFZ}-=NTF#(V^4%`++qLO{t9zot63pUrl{Y^U$T{q4%1Gp3UD3F`%{)6DlA>BQrq|r7^=RN20?|ckzfC1oiqSN=@C1Ulb=)C7# zXT(?cLMIMlBO}m>j26PaUFT7|?p|CP=!zjw=)6{l_;$SrU{kLIzPxZdDM0%*oOX{D ze7o|b@(L<{gunLZN%nGQ*jM+W^4b1w!Pou=(!M|~zf|z;ew9qlK^=BDokl}weAmyR z_zNhuztGv>t9zM5=-sZL;rJDf-@>Z>1(M^s+6yH9CE&A!JP%}6)7gevxm*Er2I#!m zwWyuOQD04LLv0P-0l_=MyTg0JckV&6%MRlH2+=h7@$}rxrok&7-@#RMrv^X9IJrvJ zu1xnGycV_bDL7fWkg4}u|Z`0r3SiLjgF8bhStlwUCAUm^KJmYl}t+38EG zs>E5m6@t_0twfl7XIXD4*){j%M!=1L8v!>0ZUo#2xDjw8;6}iWz(0w=tts7~NcHB7 zy6kd4?^5~e?uunrE)R&-blRVcrTYEpoBM0`|5G~=>=J1w{Ejznoy?T6O zGS=7AofTx~Ph|ZEGU-$otxO$iiPT-#f{Sm|{C6ZE0hN9_zVX?=aGt^8Ftu zg)eS?^bsnrGJVFg)O$1SS<_Do(f5kQ?VzKeeV}>Jqo8%~7mLS1&wWrVj)GEA+m*s| z_YUFdtMM$IJ-zxU)~B_Q(cam`cTHO{x2AFKqFa2k4pa|_%}X}ku)6lMY>(x-Xh$2o zcgFZ`RJH>jAV3w`S2b|-T`U&mo@Zok&7rFLInx>+@E)1+jj0WT)4U6+W}+9jV0~on z$2|_3FBFTsr%Je6+z7Z4a3kPGz>R<#0XG6}1l$O?5%~X$0Dp(a-`&yH8kN^fVtr%x zrh|V+@z%FzYVm?CELnb_7DkH|&)@$oRXl(DwnXv#9U!flsIdIQE5$VN7io_eg}<>o zHpS%m+d4mO2cqyda*Nf4$VCwFMN(?}rY~dy%B>hya$e8T7BZDjn?#JMh8W+Vc80icb@Kze@4xg2zEA zUM=|km*O)7kF!#IrZpZ*ag25q&r*E0@R!Bsh!bV;xnj61?z8edO8wUee%?!Q{NmuZ zBWB%;FSj{wz%KQQt3=Ib>GOM<_1B}~ zvGs@Aqtb}OQzkx1;N*wj`=m>yABP9OX7PJkf;c^lqg?;LQvt7Zy+4J%2lw|`x)4Vx z=l{IaSv)Du7l2n`kUPiQC~!}i`}LOevqW@cnP>iOddZ)sXuw#&c_8) zyG}pXN__ITS_8Za*-*~sbR%%eXK>~}v;z0bwo6YP74UncpHf}o?yrD9Q~`gi0)7&> zf3kZqTtWW=@N)SZzn1#GN%)#M4?=PkLXhM~mb4F`BYs7OEM&Cr0+F*4txj{ zR0CaUuvE$F9U@@p$T14g0EBj0j*Ecat%h889Nw5SeVAj zz%UX4JOH$jQ|^iSw=nt-NpomCp0e=#jHzGYOn83HJna$L?Rg%@l=fZhMJ*F7D2&=A z%%Qn`URN*;DowU?w*Mu_@%Fa*&+|g219(qSq3JqR*&hE1WZ8)`&E2_uob&IKO z`rtazJ8;k%f$e#o$<$9uDBOS6W7-WFtwEUQ`6$y3(%S4l>2UuQEDoSTYZkWWc`H-D zQgmKF+aE%O`o{J=zh!z{DRTe0eU9HzWxqw~@w$xZu+5%qoc50bqx+(kTND3>)qK}E zgX4ZEx8j=)d!AP_rT*EAa%=s&KL(+_eV%_aWqtO+Jk#$w?1zQul%63e@@380j zC)53og(cgu+ov4%JP%{a>wngF#_wrm&-Y*Wm8PPjie=(#{|p2aKep#}1k*KasHn63 zR~+`d-e5}CXD>W{82cS6G)Lz!@qC%{lb!m^Go^VaW<8cYuU|%207PLulbhffr}-(3 zU2dP(Ih=3Kfl)sfSpGPUkHePYXW6Sf$gvv%qWI-`;>7JTrFpR3UWAlx(4_5(Wx{qA zg|Xe9*H`Vzp7ose%+G}}>2Uw|hD^!c3ii~Nyav%45&2dQ>?K wsu#;O5U$3-*Rx0|)9*vvcLhl@yw2om9nQI3)}>`(Is4ylFg31r7&ut*4*+``?f?J) literal 0 HcmV?d00001 From 4010d9581dc1438d150a6d82b6e6871172e4c7c6 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 21 Dec 2021 00:14:41 +0900 Subject: [PATCH 29/49] flakes. --- demos/rabbit/run_clf_simulation_rabbit.m | 4 ++-- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 5664e68..b4f3d3f 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -63,7 +63,7 @@ params = init_clf_simulation_rabbit; dt = 0.025; -nstep = 5; +nstep = 10; with_slack = true; verbose_level = 1; @@ -72,7 +72,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 1.5; +plant_sys.params.scale = 1.0; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index 19296cb..45ae003 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -131,7 +131,7 @@ function Lgy = lg_y(obj, x) % TODO: Not used in RABBIT - Lgy = -1; + Lgy = zeros(obj.ydim, 1); end function LgLfy = lglf_y(obj, x) From 5d55479385302db44be1fbb0a0d650991843b4ee Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Wed, 29 Dec 2021 11:52:12 +0900 Subject: [PATCH 30/49] version updated Signed-off-by: goldenhazard --- dynsys/@RabbitBuiltIn/perturb.m | 56 +++++++++++++++++++ dynsys/@RabbitBuiltIn/reset_map.m | 17 +++++- lib/@CtrlAffineSysFL/init_sys_FL.m | 7 ++- lib/sim/rollout_controller.m | 4 +- .../rollout_controller_for_multiple_resets.m | 6 +- 5 files changed, 85 insertions(+), 5 deletions(-) create mode 100644 dynsys/@RabbitBuiltIn/perturb.m diff --git a/dynsys/@RabbitBuiltIn/perturb.m b/dynsys/@RabbitBuiltIn/perturb.m new file mode 100644 index 0000000..847820a --- /dev/null +++ b/dynsys/@RabbitBuiltIn/perturb.m @@ -0,0 +1,56 @@ +function perturbed_state = perturb(obj, state, perturbation) +%% Perturb the initial state. Useful in training script +% Perturb the initial state randomly and solve the nonlinear-op so that +% following 3 conditions are met +% Stance foot is on the ground. +% Stance foot's velocity is zero. +% Swing foot is over the ground. + +% Prevent meaningless infinity loop (zero-perturbation fault) +if perturbation == 0 + perturbed_state = state; + return; +end + +while(true) + % Initial perturbance (raw) + sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); + perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; + perturbed_state = state.* (1+perturbation_coeff); + options = optimoptions('fsolve','display','off', ... + 'Algorithm', 'levenberg-marquardt'); + % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 + constrVars = fsolve(@(z0)fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); + perturbed_state([2, 7, 8, 9]) = constrVars; + pFootSwing = p_LeftToe(perturbed_state(1:7)); + % If swing foot is under ground reconduct the perturbance. + if pFootSwing(3) > 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 diff --git a/dynsys/@RabbitBuiltIn/reset_map.m b/dynsys/@RabbitBuiltIn/reset_map.m index 75e1281..92b8af1 100644 --- a/dynsys/@RabbitBuiltIn/reset_map.m +++ b/dynsys/@RabbitBuiltIn/reset_map.m @@ -1,6 +1,21 @@ -function x_after = reset_map(obj, x_before) +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/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index 99778db..2891e4a 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -60,7 +60,12 @@ function init_sys_FL(obj, params) 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 + obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P + + A = [zeros(4), eye(4); zeros(4), zeros(4)]; + B = [zeros(4); eye(4)]; + [~, obj.Gram_clf_FL] = lqr(A, B, Q, 0.2*eye(4)); + if isfield(params, 'clf') if ~isfield(params.clf, 'rate') || ~params.clf.use_user_defined_rate diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 417c2af..0e56837 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -236,6 +236,7 @@ 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); @@ -404,7 +405,8 @@ ~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}, '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}) = {}; diff --git a/lib/sim/rollout_controller_for_multiple_resets.m b/lib/sim/rollout_controller_for_multiple_resets.m index bcda658..944f828 100644 --- a/lib/sim/rollout_controller_for_multiple_resets.m +++ b/lib/sim/rollout_controller_for_multiple_resets.m @@ -66,6 +66,7 @@ else verbose_level = settings.verbose_level; end + verbose_level_rollout = max(verbose_level-1, 0); x = x0; @@ -111,12 +112,13 @@ ts = [ts, ts_new(1:index_last)]; xs = [xs, xs_new(:, 1:index_last)]; us = [us, us_new(:, 1:index_last)]; - index_reset = [index_reset; length(ts)]; - + 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)'); From e4a008b1525d1246594c2dafdd43777a0bff7542 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sun, 9 Jan 2022 16:37:08 +0900 Subject: [PATCH 31/49] version compatible Signed-off-by: goldenhazard --- demos/rabbit/run_clf_simulation_rabbit.m | 2 +- lib/@CtrlAffineSysFL/init_sys_FL.m | 2 +- lib/sim/rollout_controller.m | 8 +++++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index b4f3d3f..2fb5b77 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -72,7 +72,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 1.0; +plant_sys.params.scale = 2.2; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index 2891e4a..9f90021 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -60,7 +60,7 @@ function init_sys_FL(obj, params) 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 + %obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P A = [zeros(4), eye(4); zeros(4), zeros(4)]; B = [zeros(4); eye(4)]; diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 0e56837..5adbee3 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -217,11 +217,15 @@ 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) @@ -258,7 +262,9 @@ 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]; From 1336f748581f692bd44eec17a531c15c5a758c92 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Fri, 14 Jan 2022 13:13:59 +0900 Subject: [PATCH 32/49] perturb incorporated in RabbitBuiltIn Method Signed-off-by: goldenhazard --- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 54 ++++++++++++++++++++++++++ dynsys/@RabbitBuiltIn/perturb.m | 56 --------------------------- lib/@CtrlAffineSysFL/init_sys_FL.m | 6 +-- 3 files changed, 57 insertions(+), 59 deletions(-) delete mode 100644 dynsys/@RabbitBuiltIn/perturb.m diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index 45ae003..4e19f91 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -722,6 +722,60 @@ % % function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) % end + function perturbed_state = perturb(obj, state, perturbation) + %% Perturb the initial state. Useful in training script + % Perturb the initial state randomly and solve the nonlinear-op so that + % following 3 conditions are met + % Stance foot is on the ground. + % Stance foot's velocity is zero. + % Swing foot is over the ground. + + % Prevent meaningless infinity loop (zero-perturbation fault) + if perturbation == 0 + perturbed_state = state; + return; + end + + while(true) + % Initial perturbance (raw) + sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); + perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; + perturbed_state = state.* (1+perturbation_coeff); + options = optimoptions('fsolve','display','off', ... + 'Algorithm', 'levenberg-marquardt'); + % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 + constrVars = fsolve(@(z0) obj.fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); + perturbed_state([2, 7, 8, 9]) = constrVars; + pFootSwing = p_LeftToe(perturbed_state(1:7)); + % If swing foot is under ground reconduct the perturbance. + if pFootSwing(3) > 0 + break + end + end + end + function constr= fzeroFunc(obj, 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 end end diff --git a/dynsys/@RabbitBuiltIn/perturb.m b/dynsys/@RabbitBuiltIn/perturb.m deleted file mode 100644 index 847820a..0000000 --- a/dynsys/@RabbitBuiltIn/perturb.m +++ /dev/null @@ -1,56 +0,0 @@ -function perturbed_state = perturb(obj, state, perturbation) -%% Perturb the initial state. Useful in training script -% Perturb the initial state randomly and solve the nonlinear-op so that -% following 3 conditions are met -% Stance foot is on the ground. -% Stance foot's velocity is zero. -% Swing foot is over the ground. - -% Prevent meaningless infinity loop (zero-perturbation fault) -if perturbation == 0 - perturbed_state = state; - return; -end - -while(true) - % Initial perturbance (raw) - sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); - perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; - perturbed_state = state.* (1+perturbation_coeff); - options = optimoptions('fsolve','display','off', ... - 'Algorithm', 'levenberg-marquardt'); - % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 - constrVars = fsolve(@(z0)fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); - perturbed_state([2, 7, 8, 9]) = constrVars; - pFootSwing = p_LeftToe(perturbed_state(1:7)); - % If swing foot is under ground reconduct the perturbance. - if pFootSwing(3) > 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 diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index 9f90021..bb4f2fc 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -62,9 +62,9 @@ function init_sys_FL(obj, params) -Kp*eye(obj.ydim), -Kd*eye(obj.ydim)]; %obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P - A = [zeros(4), eye(4); zeros(4), zeros(4)]; - B = [zeros(4); eye(4)]; - [~, obj.Gram_clf_FL] = lqr(A, B, Q, 0.2*eye(4)); +% A = [zeros(4), eye(4); zeros(4), zeros(4)]; +% B = [zeros(4); eye(4)]; +% [~, obj.Gram_clf_FL] = lqr(A, B, Q, 0.2*eye(4)); if isfield(params, 'clf') From 2691ab7584c842e8f7f71eb01ebc434ff5d6c5f2 Mon Sep 17 00:00:00 2001 From: jason Date: Thu, 13 Jan 2022 22:35:28 -0800 Subject: [PATCH 33/49] change file path. --- .../Ce_five_link_walker.mexa64 | Bin .../Ce_five_link_walker.mexmaci64 | Bin .../{Rabbit_helper => }/Ce_five_link_walker.mexw64 | Bin .../De_five_link_walker.mexa64 | Bin .../De_five_link_walker.mexmaci64 | Bin .../{Rabbit_helper => }/De_five_link_walker.mexw64 | Bin .../Ge_five_link_walker.mexa64 | Bin .../Ge_five_link_walker.mexmaci64 | Bin .../{Rabbit_helper => }/Ge_five_link_walker.mexw64 | Bin .../J_LeftToe.mexa64 | Bin .../{Rabbit_helper => }/J_LeftToe.mexmaci64 | Bin .../{Rabbit_helper => }/J_LeftToe.mexw64 | Bin .../J_RightToe.mexa64 | Bin .../{Rabbit_helper => }/J_RightToe.mexmaci64 | Bin .../{Rabbit_helper => }/J_RightToe.mexw64 | Bin .../dJ_LeftToe.mexa64 | Bin .../{Rabbit_helper => }/dJ_LeftToe.mexmaci64 | Bin .../dJ_RightToe.mexa64 | Bin .../{Rabbit_helper => }/dJ_RightToe.mexmaci64 | Bin .../{Rabbit_helper => }/dJ_RightToe.mexw64 | Bin .../p_LeftToe.mexa64 | Bin .../{Rabbit_helper => }/p_LeftToe.mexmaci64 | Bin .../{Rabbit_helper => }/p_LeftToe.mexw64 | Bin .../p_RightToe.mexa64 | Bin .../{Rabbit_helper => }/p_RightToe.mexmaci64 | Bin .../{Rabbit_helper => }/p_RightToe.mexw64 | Bin .../p_Torso.mexa64 | Bin .../{Rabbit_helper => }/p_Torso.mexmaci64 | Bin .../{Rabbit_helper => }/p_Torso.mexw64 | Bin .../p_q2_left.mexa64 | Bin .../{Rabbit_helper => }/p_q2_left.mexmaci64 | Bin .../{Rabbit_helper => }/p_q2_left.mexw64 | Bin .../p_q2_right.mexa64 | Bin .../{Rabbit_helper => }/p_q2_right.mexmaci64 | Bin .../{Rabbit_helper => }/p_q2_right.mexw64 | Bin .../{Rabbit_helper => }/trajectory_data.mat | Bin 36 files changed, 0 insertions(+), 0 deletions(-) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/Ce_five_link_walker.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/Ce_five_link_walker.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/Ce_five_link_walker.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/De_five_link_walker.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/De_five_link_walker.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/De_five_link_walker.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/Ge_five_link_walker.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/Ge_five_link_walker.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/Ge_five_link_walker.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/J_LeftToe.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/J_LeftToe.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/J_LeftToe.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/J_RightToe.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/J_RightToe.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/J_RightToe.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/dJ_LeftToe.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/dJ_LeftToe.mexmaci64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/dJ_RightToe.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/dJ_RightToe.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/dJ_RightToe.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/p_LeftToe.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_LeftToe.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_LeftToe.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/p_RightToe.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_RightToe.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_RightToe.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/p_Torso.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_Torso.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_Torso.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/p_q2_left.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_q2_left.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_q2_left.mexw64 (100%) rename demos/rabbit/{rabbit_helper_ubuntu => rabbit_helper}/p_q2_right.mexa64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_q2_right.mexmaci64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/p_q2_right.mexw64 (100%) rename demos/rabbit/rabbit_helper/{Rabbit_helper => }/trajectory_data.mat (100%) diff --git a/demos/rabbit/rabbit_helper_ubuntu/Ce_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/Ce_five_link_walker.mexa64 rename to demos/rabbit/rabbit_helper/Ce_five_link_walker.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexmaci64 rename to demos/rabbit/rabbit_helper/Ce_five_link_walker.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Ce_five_link_walker.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/Ce_five_link_walker.mexw64 rename to demos/rabbit/rabbit_helper/Ce_five_link_walker.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/De_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper/De_five_link_walker.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/De_five_link_walker.mexa64 rename to demos/rabbit/rabbit_helper/De_five_link_walker.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/De_five_link_walker.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexmaci64 rename to demos/rabbit/rabbit_helper/De_five_link_walker.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/De_five_link_walker.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/De_five_link_walker.mexw64 rename to demos/rabbit/rabbit_helper/De_five_link_walker.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/Ge_five_link_walker.mexa64 b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/Ge_five_link_walker.mexa64 rename to demos/rabbit/rabbit_helper/Ge_five_link_walker.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexmaci64 b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexmaci64 rename to demos/rabbit/rabbit_helper/Ge_five_link_walker.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexw64 b/demos/rabbit/rabbit_helper/Ge_five_link_walker.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/Ge_five_link_walker.mexw64 rename to demos/rabbit/rabbit_helper/Ge_five_link_walker.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/J_LeftToe.mexa64 b/demos/rabbit/rabbit_helper/J_LeftToe.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/J_LeftToe.mexa64 rename to demos/rabbit/rabbit_helper/J_LeftToe.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/J_LeftToe.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexmaci64 rename to demos/rabbit/rabbit_helper/J_LeftToe.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexw64 b/demos/rabbit/rabbit_helper/J_LeftToe.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/J_LeftToe.mexw64 rename to demos/rabbit/rabbit_helper/J_LeftToe.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/J_RightToe.mexa64 b/demos/rabbit/rabbit_helper/J_RightToe.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/J_RightToe.mexa64 rename to demos/rabbit/rabbit_helper/J_RightToe.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/J_RightToe.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexmaci64 rename to demos/rabbit/rabbit_helper/J_RightToe.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexw64 b/demos/rabbit/rabbit_helper/J_RightToe.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/J_RightToe.mexw64 rename to demos/rabbit/rabbit_helper/J_RightToe.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/dJ_LeftToe.mexa64 b/demos/rabbit/rabbit_helper/dJ_LeftToe.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/dJ_LeftToe.mexa64 rename to demos/rabbit/rabbit_helper/dJ_LeftToe.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/dJ_LeftToe.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/dJ_LeftToe.mexmaci64 rename to demos/rabbit/rabbit_helper/dJ_LeftToe.mexmaci64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/dJ_RightToe.mexa64 b/demos/rabbit/rabbit_helper/dJ_RightToe.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/dJ_RightToe.mexa64 rename to demos/rabbit/rabbit_helper/dJ_RightToe.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/dJ_RightToe.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexmaci64 rename to demos/rabbit/rabbit_helper/dJ_RightToe.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexw64 b/demos/rabbit/rabbit_helper/dJ_RightToe.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/dJ_RightToe.mexw64 rename to demos/rabbit/rabbit_helper/dJ_RightToe.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_LeftToe.mexa64 b/demos/rabbit/rabbit_helper/p_LeftToe.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/p_LeftToe.mexa64 rename to demos/rabbit/rabbit_helper/p_LeftToe.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexmaci64 b/demos/rabbit/rabbit_helper/p_LeftToe.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexmaci64 rename to demos/rabbit/rabbit_helper/p_LeftToe.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexw64 b/demos/rabbit/rabbit_helper/p_LeftToe.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_LeftToe.mexw64 rename to demos/rabbit/rabbit_helper/p_LeftToe.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_RightToe.mexa64 b/demos/rabbit/rabbit_helper/p_RightToe.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/p_RightToe.mexa64 rename to demos/rabbit/rabbit_helper/p_RightToe.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexmaci64 b/demos/rabbit/rabbit_helper/p_RightToe.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexmaci64 rename to demos/rabbit/rabbit_helper/p_RightToe.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexw64 b/demos/rabbit/rabbit_helper/p_RightToe.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_RightToe.mexw64 rename to demos/rabbit/rabbit_helper/p_RightToe.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_Torso.mexa64 b/demos/rabbit/rabbit_helper/p_Torso.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/p_Torso.mexa64 rename to demos/rabbit/rabbit_helper/p_Torso.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexmaci64 b/demos/rabbit/rabbit_helper/p_Torso.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexmaci64 rename to demos/rabbit/rabbit_helper/p_Torso.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexw64 b/demos/rabbit/rabbit_helper/p_Torso.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_Torso.mexw64 rename to demos/rabbit/rabbit_helper/p_Torso.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_q2_left.mexa64 b/demos/rabbit/rabbit_helper/p_q2_left.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/p_q2_left.mexa64 rename to demos/rabbit/rabbit_helper/p_q2_left.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexmaci64 b/demos/rabbit/rabbit_helper/p_q2_left.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexmaci64 rename to demos/rabbit/rabbit_helper/p_q2_left.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexw64 b/demos/rabbit/rabbit_helper/p_q2_left.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_left.mexw64 rename to demos/rabbit/rabbit_helper/p_q2_left.mexw64 diff --git a/demos/rabbit/rabbit_helper_ubuntu/p_q2_right.mexa64 b/demos/rabbit/rabbit_helper/p_q2_right.mexa64 similarity index 100% rename from demos/rabbit/rabbit_helper_ubuntu/p_q2_right.mexa64 rename to demos/rabbit/rabbit_helper/p_q2_right.mexa64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexmaci64 b/demos/rabbit/rabbit_helper/p_q2_right.mexmaci64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexmaci64 rename to demos/rabbit/rabbit_helper/p_q2_right.mexmaci64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexw64 b/demos/rabbit/rabbit_helper/p_q2_right.mexw64 similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/p_q2_right.mexw64 rename to demos/rabbit/rabbit_helper/p_q2_right.mexw64 diff --git a/demos/rabbit/rabbit_helper/Rabbit_helper/trajectory_data.mat b/demos/rabbit/rabbit_helper/trajectory_data.mat similarity index 100% rename from demos/rabbit/rabbit_helper/Rabbit_helper/trajectory_data.mat rename to demos/rabbit/rabbit_helper/trajectory_data.mat From 25536847fa199fb2272035638eb9ced1f0794d61 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sun, 16 Jan 2022 15:50:02 +0900 Subject: [PATCH 34/49] gen replaced separated in other file Signed-off-by: goldenhazard --- demos/rabbit/run_clf_simulation_rabbit.m | 2 +- dynsys/@RabbitBuiltIn/C_gen_rel.m | 71 ++ dynsys/@RabbitBuiltIn/D_gen_rel.m | 86 ++ dynsys/@RabbitBuiltIn/G_gen_rel.m | 60 ++ dynsys/@RabbitBuiltIn/L2ydq_a_gen.m | 8 + dynsys/@RabbitBuiltIn/L2ydq_d_gen.m | 193 ++++ dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 1160 +++++++++++----------- dynsys/@RabbitBuiltIn/dq_pos_gen_v2.m | 20 + dynsys/@RabbitBuiltIn/dydq_a_gen.m | 9 + dynsys/@RabbitBuiltIn/dydq_d_gen.m | 78 ++ dynsys/@RabbitBuiltIn/perturb.m | 54 + lib/@CtrlAffineSysFL/init_sys_FL.m | 2 +- 12 files changed, 1161 insertions(+), 582 deletions(-) create mode 100644 dynsys/@RabbitBuiltIn/C_gen_rel.m create mode 100644 dynsys/@RabbitBuiltIn/D_gen_rel.m create mode 100644 dynsys/@RabbitBuiltIn/G_gen_rel.m create mode 100644 dynsys/@RabbitBuiltIn/L2ydq_a_gen.m create mode 100644 dynsys/@RabbitBuiltIn/L2ydq_d_gen.m create mode 100644 dynsys/@RabbitBuiltIn/dq_pos_gen_v2.m create mode 100644 dynsys/@RabbitBuiltIn/dydq_a_gen.m create mode 100644 dynsys/@RabbitBuiltIn/dydq_d_gen.m create mode 100644 dynsys/@RabbitBuiltIn/perturb.m diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 2fb5b77..ccf7929 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -72,7 +72,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 2.2; +plant_sys.params.scale = 1.8; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... 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 index 4e19f91..d60a243 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -167,532 +167,532 @@ L2fy = (L2ydq_act - L2ydq_des)*f_s; end - %% Generator - 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 - 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 - 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 - 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 - 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 - 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 - 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 - 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 +% %% Generator +% 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 +% 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 +% 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 +% 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 +% 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 +% 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 +% 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 +% 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 % function y_max_exceed_ = y_max_exceed(obj, x) % end % @@ -722,60 +722,60 @@ % % function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) % end - function perturbed_state = perturb(obj, state, perturbation) - %% Perturb the initial state. Useful in training script - % Perturb the initial state randomly and solve the nonlinear-op so that - % following 3 conditions are met - % Stance foot is on the ground. - % Stance foot's velocity is zero. - % Swing foot is over the ground. - - % Prevent meaningless infinity loop (zero-perturbation fault) - if perturbation == 0 - perturbed_state = state; - return; - end - - while(true) - % Initial perturbance (raw) - sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); - perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; - perturbed_state = state.* (1+perturbation_coeff); - options = optimoptions('fsolve','display','off', ... - 'Algorithm', 'levenberg-marquardt'); - % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 - constrVars = fsolve(@(z0) obj.fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); - perturbed_state([2, 7, 8, 9]) = constrVars; - pFootSwing = p_LeftToe(perturbed_state(1:7)); - % If swing foot is under ground reconduct the perturbance. - if pFootSwing(3) > 0 - break - end - end - end - function constr= fzeroFunc(obj, 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 +% function perturbed_state = perturb(obj, state, perturbation) +% %% Perturb the initial state. Useful in training script +% % Perturb the initial state randomly and solve the nonlinear-op so that +% % following 3 conditions are met +% % Stance foot is on the ground. +% % Stance foot's velocity is zero. +% % Swing foot is over the ground. +% +% % Prevent meaningless infinity loop (zero-perturbation fault) +% if perturbation == 0 +% perturbed_state = state; +% return; +% end +% +% while(true) +% % Initial perturbance (raw) +% sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); +% perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; +% perturbed_state = state.* (1+perturbation_coeff); +% options = optimoptions('fsolve','display','off', ... +% 'Algorithm', 'levenberg-marquardt'); +% % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 +% constrVars = fsolve(@(z0) obj.fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); +% perturbed_state([2, 7, 8, 9]) = constrVars; +% pFootSwing = p_LeftToe(perturbed_state(1:7)); +% % If swing foot is under ground reconduct the perturbance. +% if pFootSwing(3) > 0 +% break +% end +% end +% end +% function constr= fzeroFunc(obj, 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 end end 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/perturb.m b/dynsys/@RabbitBuiltIn/perturb.m new file mode 100644 index 0000000..9a13113 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/perturb.m @@ -0,0 +1,54 @@ +function perturbed_state = perturb(obj, state, perturbation) +%% Perturb the initial state. Useful in training script +% Perturb the initial state randomly and solve the nonlinear-op so that +% following 3 conditions are met +% Stance foot is on the ground. +% Stance foot's velocity is zero. +% Swing foot is over the ground. + +% Prevent meaningless infinity loop (zero-perturbation fault) +if perturbation == 0 + perturbed_state = state; + return; +end + +while(true) + % Initial perturbance (raw) + sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); + perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; + perturbed_state = state.* (1+perturbation_coeff); + options = optimoptions('fsolve','display','off', ... + 'Algorithm', 'levenberg-marquardt'); + % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 + constrVars = fsolve(@(z0) fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); + perturbed_state([2, 7, 8, 9]) = constrVars; + pFootSwing = p_LeftToe(perturbed_state(1:7)); + % If swing foot is under ground reconduct the perturbance. + if pFootSwing(3) > 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/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index bb4f2fc..5aa7a87 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -60,7 +60,7 @@ function init_sys_FL(obj, params) 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 + obj.Gram_clf_FL = lyap(A', Q); % A'P+PA=Q, find P % A = [zeros(4), eye(4); zeros(4), zeros(4)]; % B = [zeros(4); eye(4)]; From 076b1881cea45f07aa83ad26c975b4ac64fb9010 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sun, 16 Jan 2022 15:53:23 +0900 Subject: [PATCH 35/49] trimmed Signed-off-by: goldenhazard --- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 610 -------------------------- 1 file changed, 610 deletions(-) diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index d60a243..c10af67 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -166,616 +166,6 @@ L2fy = (L2ydq_act - L2ydq_des)*f_s; end - -% %% Generator -% 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 -% 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 -% 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 -% 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 -% 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 -% 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 -% 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 -% 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 -% function y_max_exceed_ = y_max_exceed(obj, x) -% end -% -% function lf_y_max_exceed_ = lf_y_max_exceed(obj, x) -% end -% -% function lg_y_max_exceed_ = lg_y_max_exceed(obj, x) -% end -% -% function lglf_y_max_exceed_ = lglf_y_max_exceed(obj, x) -% end -% -% function l2f_y_max_exceed_ = l2f_y_max_exceed(obj, x) -% end -% -% function y_min_exceed_ = y_min_exceed(obj, x) -% end -% -% function lf_y_min_exceed_ = lf_y_min_exceed(obj, x) -% end -% -% function lg_y_min_exceed_ = lg_y_min_exceed(obj, x) -% end -% -% function lglf_y_min_exceed_ = lglf_y_min_exceed(obj, x) -% end -% -% function l2f_y_min_exceed_ = l2f_y_min_exceed(obj, x) -% end -% function perturbed_state = perturb(obj, state, perturbation) -% %% Perturb the initial state. Useful in training script -% % Perturb the initial state randomly and solve the nonlinear-op so that -% % following 3 conditions are met -% % Stance foot is on the ground. -% % Stance foot's velocity is zero. -% % Swing foot is over the ground. -% -% % Prevent meaningless infinity loop (zero-perturbation fault) -% if perturbation == 0 -% perturbed_state = state; -% return; -% end -% -% while(true) -% % Initial perturbance (raw) -% sign_vector = 2*((rand(obj.xdim, 1) < 0.5) - 0.5); -% perturbation_coeff = rand(obj.xdim, 1).* sign_vector*perturbation; -% perturbed_state = state.* (1+perturbation_coeff); -% options = optimoptions('fsolve','display','off', ... -% 'Algorithm', 'levenberg-marquardt'); -% % Nonlinear optimize so that p_RightToe(3) = 0, v_RightToe(3)=0 -% constrVars = fsolve(@(z0) obj.fzeroFunc(z0,perturbed_state), perturbed_state([2, 7, 8, 9]),options); -% perturbed_state([2, 7, 8, 9]) = constrVars; -% pFootSwing = p_LeftToe(perturbed_state(1:7)); -% % If swing foot is under ground reconduct the perturbance. -% if pFootSwing(3) > 0 -% break -% end -% end -% end -% function constr= fzeroFunc(obj, 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 end end From 4bb3c617772a90f7076a229ddfdb0dd14c2d7606 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Tue, 25 Jan 2022 23:46:41 +0900 Subject: [PATCH 36/49] rl-clf reviewed version Signed-off-by: goldenhazard --- demos/rabbit/run_clf_simulation_rabbit.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index ccf7929..829d463 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -63,6 +63,7 @@ params = init_clf_simulation_rabbit; dt = 0.025; +%dt = 0.002; nstep = 10; with_slack = true; verbose_level = 1; @@ -72,7 +73,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 1.8; +plant_sys.params.scale = 1.7; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... From d0021bbd30b0a95da0eebaa4aa58bfb37b2a6c13 Mon Sep 17 00:00:00 2001 From: jason Date: Wed, 2 Feb 2022 00:06:57 -0800 Subject: [PATCH 37/49] remove dependency on 'params' and refactor 'ctrlClfQpFL' --- demos/rabbit/init_clf_simulation_rabbit.m | 4 - demos/rabbit/plot_rabbit_result.m | 4 +- demos/rabbit/run_clf_simulation_rabbit.m | 2 +- lib/@CtrlAffineSys/init_sys.m | 2 - lib/@CtrlAffineSysFL/CtrlAffineSysFL.m | 13 +- lib/@CtrlAffineSysFL/ctrlClfQpFL.m | 117 ++++++----------- lib/@CtrlAffineSysFL/init_sys_FL.m | 145 +++++++++++++++++----- 7 files changed, 161 insertions(+), 126 deletions(-) diff --git a/demos/rabbit/init_clf_simulation_rabbit.m b/demos/rabbit/init_clf_simulation_rabbit.m index c20e465..d2a0c79 100644 --- a/demos/rabbit/init_clf_simulation_rabbit.m +++ b/demos/rabbit/init_clf_simulation_rabbit.m @@ -41,10 +41,6 @@ %% Control Barrier Function Parameters dict.cbf.rate = 3; dict.cbf_gamma0 = 1; - - %% Control Lyapunov Function Parameters - dict.clf.rate = 0.7; - dict.clf.use_user_defined_rate = false; % set this to true if you don't want to use % Be careful when tuning this! Low epsilon induces high performance, % but it can be too aggresive to disregard some physical limitations diff --git a/demos/rabbit/plot_rabbit_result.m b/demos/rabbit/plot_rabbit_result.m index f1659cb..84362e0 100644 --- a/demos/rabbit/plot_rabbit_result.m +++ b/demos/rabbit/plot_rabbit_result.m @@ -54,14 +54,14 @@ function plot_rabbit_result(xs, ts, us, extras) 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); +% 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); +% callPlotSettings(fig_sz_with_subplots, plot_pos); %print('-f9','plot_tracking_error', '-dpng'); figure(7); diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 829d463..9fd992e 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -73,7 +73,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 1.7; +plant_sys.params.scale = 1.0; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 722d6f5..20ebdd5 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -124,7 +124,6 @@ function init_sys(obj, params) %% 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; @@ -133,7 +132,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; diff --git a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m index d5e783c..83e5b93 100644 --- a/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m +++ b/lib/@CtrlAffineSysFL/CtrlAffineSysFL.m @@ -34,8 +34,11 @@ % 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 @@ -55,10 +58,6 @@ obj@CtrlAffineSys(params, setup_option); obj.init_sys_FL(params); - - %[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); end function [y, phase, y_max_exceed, y_min_exceed] = defineOutput(obj, params, symbolic_state) @@ -69,18 +68,18 @@ 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 diff --git a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m index f6ac107..f771773 100644 --- a/lib/@CtrlAffineSysFL/ctrlClfQpFL.m +++ b/lib/@CtrlAffineSysFL/ctrlClfQpFL.m @@ -29,17 +29,22 @@ end if ~isfield(kwargs, 'verbose') % Run QP without log in default condition. - verbose = 1; + 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 tstart = tic; - [y, dy, L2fy, LgLfy, ~] = obj.eval_y(x); V = obj.clf_FL(y, dy); @@ -48,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 + A_slack = -1; + if ~isempty(obj.u_max) + A_slack = [A_slack; zeros(obj.udim, 1)]; 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 + if ~isempty(obj.u_min) + A_slack = [A_slack; zeros(obj.udim, 1)]; 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 - 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.") - end - else - weight_input = eye(obj.udim); + A = [A, A_slack]; end if verbose @@ -122,33 +81,39 @@ 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; if verbose - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + 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; if verbose - disp("Infeasible QP. CBF constraint is conflicting with input constraints."); + 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 diff --git a/lib/@CtrlAffineSysFL/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index 5aa7a87..a7040e1 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -4,6 +4,10 @@ function init_sys_FL(obj, params) obj.use_phase = 1; 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 @@ -26,10 +30,18 @@ function init_sys_FL(obj, params) end %% Set up desired linear output dynamics %% Both Symbolic and Non-symbolic - if ~isfield(params, 'epsilon_FL') - error("Define variable in the params called 'epsilon_FL', which is the rate of RES-CLF"); + 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 - eps = params.epsilon_FL; + 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); @@ -41,40 +53,105 @@ function init_sys_FL(obj, params) % In controlSysFL, CLF is automatically set, so obj.n_clf can be % hard-coded to 1 obj.n_clf = 1; - 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); + %% 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 - if size(params.Q_FL, 1) ~= 2*obj.ydim - error("Wrong Q_FL size.") - end - Q = params.Q_FL; + clf_type = 3; 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 -% A = [zeros(4), eye(4); zeros(4), zeros(4)]; -% B = [zeros(4); eye(4)]; -% [~, obj.Gram_clf_FL] = lqr(A, B, Q, 0.2*eye(4)); - - - if isfield(params, 'clf') - if ~isfield(params.clf, 'rate') || ~params.clf.use_user_defined_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); + 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.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); + 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 - +end \ No newline at end of file From efc607984bd0a345be6933afcbd0ff3afcac277f Mon Sep 17 00:00:00 2001 From: jason Date: Sun, 6 Feb 2022 00:21:19 -0800 Subject: [PATCH 38/49] fix small bugs. --- lib/@CtrlAffineSys/ctrlCbfClfQp.m | 4 ++-- lib/@CtrlAffineSys/init_sys.m | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/lib/@CtrlAffineSys/ctrlCbfClfQp.m b/lib/@CtrlAffineSys/ctrlCbfClfQp.m index 21952cc..affa391 100644 --- a/lib/@CtrlAffineSys/ctrlCbfClfQp.m +++ b/lib/@CtrlAffineSys/ctrlCbfClfQp.m @@ -73,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)]; @@ -90,7 +90,7 @@ A = [A, A_slack]; end if ~isfield(kwargs, 'weight_slack') - weight_slack = obj.weight_slack * ones(n_slack); + weight_slack = obj.weight_slack * ones(n_slack, 1); else if numel(kwargs.weight_slack) ~= n_slack error("wrong weight_slack size. it should be a vector of n_slack:=%s", ... diff --git a/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 20ebdd5..9939e15 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -106,6 +106,7 @@ function init_sys(obj, params) n_clf = 1; try clf_test = obj.clf(x_test); + n_clf = length(clf_test); catch e n_clf = 0; end @@ -113,6 +114,7 @@ function init_sys(obj, params) n_cbf = 1; try cbf_test = obj.cbf(x_test); + n_cbf = length(cbf_test); catch e n_cbf = 0; end From 3de33423d7c2d352f79bf122949ebf46b2167302 Mon Sep 17 00:00:00 2001 From: jason Date: Sun, 6 Feb 2022 00:23:34 -0800 Subject: [PATCH 39/49] add rabbit stepping stone demo. --- demos/rabbit/anim/anim_moving_stone_load.m | 10 +- demos/rabbit/init_clf_simulation_rabbit.m | 101 ++++------ demos/rabbit/run_clf_simulation_rabbit.m | 14 +- demos/rabbit/run_stepping_stone_rabbit.m | 130 ++++++++++++ dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 113 ++++++----- dynsys/@RabbitBuiltIn/cbf.m | 44 ++++ ...amics_matrices_in_reduced_relative_coord.m | 89 +++++++++ dynsys/@RabbitBuiltIn/get_hs.m | 34 ++++ dynsys/@RabbitBuiltIn/lf_cbf.m | 58 ++++++ dynsys/@RabbitBuiltIn/lg_cbf.m | 44 ++++ .../reset_map_with_step_update.m | 29 +++ lib/@CtrlAffineSysFL/ctrlCbfClfQpFL.m | 189 +++++++++--------- lib/@CtrlAffineSysFL/init_sys_FL.m | 5 +- 13 files changed, 640 insertions(+), 220 deletions(-) create mode 100644 demos/rabbit/run_stepping_stone_rabbit.m create mode 100644 dynsys/@RabbitBuiltIn/cbf.m create mode 100644 dynsys/@RabbitBuiltIn/get_dynamics_matrices_in_reduced_relative_coord.m create mode 100644 dynsys/@RabbitBuiltIn/get_hs.m create mode 100644 dynsys/@RabbitBuiltIn/lf_cbf.m create mode 100644 dynsys/@RabbitBuiltIn/lg_cbf.m create mode 100644 dynsys/@RabbitBuiltIn/reset_map_with_step_update.m diff --git a/demos/rabbit/anim/anim_moving_stone_load.m b/demos/rabbit/anim/anim_moving_stone_load.m index d1e50b6..5295fc8 100644 --- a/demos/rabbit/anim/anim_moving_stone_load.m +++ b/demos/rabbit/anim/anim_moving_stone_load.m @@ -23,9 +23,9 @@ function anim_moving_stone_load(t,x,ts,l_min_t,l_max_t) %save as a video -spwriter = VideoWriter('biped_walking.avi'); -set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); -open(spwriter); +% spwriter = VideoWriter('biped_walking.avi'); +% set(spwriter, 'FrameRate', 0.3/ts,'Quality',100); +% open(spwriter); fig1 = figure(1); set(0,'Units','pixels') @@ -70,8 +70,8 @@ function anim_moving_stone_load(t,x,ts,l_min_t,l_max_t) hold on; pause(ts); %pause(0.5); - frame = getframe(gcf); - writeVideo(spwriter, frame); +% frame = getframe(gcf); +% writeVideo(spwriter, frame); end end diff --git a/demos/rabbit/init_clf_simulation_rabbit.m b/demos/rabbit/init_clf_simulation_rabbit.m index d2a0c79..485b5fc 100644 --- a/demos/rabbit/init_clf_simulation_rabbit.m +++ b/demos/rabbit/init_clf_simulation_rabbit.m @@ -1,60 +1,45 @@ %% Initialize Global Simulation Configuration of Rabbit -function dict = init_clf_simulation_rabbit - %% Aggregate simulation setting in the params.config - % TODO: we should classify some notions into certain groups, mixed ugly - % now! - - %% Control Mode: You can use various control options - dict.mode.motor_dynamics = 1; % going to be supported - dict.mode.apply_rl_adaptation = 0; % going to be supported - % 0: Original(Westervelt) 1: CLF-QP 2: L1-CLF-QP 3: RL-CLF-QP - % IO-RL+CLF: apply_rl_adaptation=0, control_type=1 - dict.mode.control_type = 1; % going to be supported - - %% Physical Property - dict.physics.mu = 0.8; % friction constraint; - - %% Legacy : Trajectory optimization legacy, Initial setting - % This is derived from the code Rabbit-IO-RL code - % This is delicate, keep it fixed - dict.legacy.theta_end = 3.384545423940834; - dict.legacy.theta_init = 2.903323878938605; - dict.legacy.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]; - dict.legacy.q0_ini = [-0.161463255651884;0.739782734886561;0.152681626367748;2.44507104015569;0.658106151373543;2.95111227794945;0.599243461678844]; - dict.legacy.dq0_ini = [0.781190436522269;0.243063912942872;0.247793330530586;1.10287289727466;-0.548512132037030;0.203061891957879;1.50818729243256]; - - %% Fulfill params - dict.xdim = length(dict.legacy.q0_ini) + length(dict.legacy.dq0_ini); - dict.udim = 4; % TODO hard-coding - dict.rel_deg_y = 2; - dict.use_phase = false; - - % Uncertainty term - dict.scale = 1; - dict.torso_add = 0; - - %% Input options - dict.uSat = 150; - dict.u_max = dict.uSat; - dict.u_min = -dict.uSat; - %% Control Barrier Function Parameters - dict.cbf.rate = 3; - dict.cbf_gamma0 = 1; - - % Be careful when tuning this! Low epsilon induces high performance, - % but it can be too aggresive to disregard some physical limitations - % and unknown dynamics - dict.epsilon_FL = 0.07; % Keep it fixed! Fragile! - dict.Kp_FL = 1; - dict.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 - dict.weight.input = 1; % A (can be scalar or matrix) - dict.weight.slack = 100; % B (should be scalar) - - %% Robot Fall Sensor - dict.fall_threshold = 0.699; -end \ No newline at end of file +%% 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/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 9fd992e..1b553b7 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -61,7 +61,7 @@ % 0: use slack variable while solving QP % 1: not use slack variable while solving QP -params = init_clf_simulation_rabbit; +init_clf_simulation_rabbit; dt = 0.025; %dt = 0.002; nstep = 10; @@ -84,8 +84,6 @@ %% Step 3. Initialize state % Initialize the state vector. -q0_ini = params.legacy.q0_ini; -dq0_ini = params.legacy.dq0_ini; x0 = [q0_ini;dq0_ini]; x = x0; t0 = 0; @@ -105,8 +103,8 @@ %% 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 +% 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..8a3af40 --- /dev/null +++ b/demos/rabbit/run_stepping_stone_rabbit.m @@ -0,0 +1,130 @@ +%% 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; +%% 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); +xs = xs'; +ts = ts'; + +l_min_t_anim = []; +l_max_t_anim = []; +index_reset = [0; extras.index_reset]; +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_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)]; +end +%% 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; +global SimConfig +SimConfig.m_load=0; +animation_dt = 0.05; +s_quan_vec = coordinateTransformation(xs); +anim_moving_stone_load(ts, s_quan_vec, animation_dt, l_min_t_anim, l_max_t_anim); \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index c10af67..d40fa18 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -12,11 +12,46 @@ 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; + obj.gamma_b = obj.params.gamma_b; + 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) @@ -25,18 +60,7 @@ q = x(1:n); dq = x(n+1:2*n); - %[D, C, G, B] = obj.gen_DCGB_rel(q); - %D = D_gen_rel(q, obj.params.scale, obj.params.torso_add); % 7 x 7 - 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]; + [D, C, G, B] = obj.get_dynamics_matrices(x); JSt = J_RightToe(q); JSt = JSt([1,3],:); dJSt = dJ_RightToe(q,dq); @@ -74,24 +98,11 @@ q = xs(1:n, :); dq = xs(n+1:2*n, :); - theta = q(3, :) + q(4, :) + q(5, :)/2; -% theta_init = obj.params.legacy.theta_init; -% theta_end = obj.params.legacy.theta_end; -% phase = min(max((theta - theta_init)/(theta_end - theta_init))); - + theta = q(3, :) + q(4, :) + q(5, :)/2; dtheta = dq(3, :) + dq(4, :) + dq(5, :)/2; zs = [theta; dtheta]; end - function B = cbf(obj, x) - % TODO: CBF(x) - end - function LfB = lf_cbf(obj, x) - % TODO: LfB(x) - end - function LgB = lg_cbf(obj, x) - % TODO: LgB(x) - end function y_ = y(obj, x) % TODO: Rabbit's output @@ -99,12 +110,9 @@ q = x(1:n); theta = q(3) + q(4) + q(5)/2; - a_bez = obj.params.legacy.a_bez; - theta_init = obj.params.legacy.theta_init; - theta_end = obj.params.legacy.theta_end; - phase = min(max((theta - theta_init)/(theta_end - theta_init))); + phase = min(max((theta - obj.theta_init)/(obj.theta_end - obj.theta_init))); - yd = obj.bezier_outputs(a_bez, phase); % Not very structured actually + yd = obj.bezier_outputs(obj.a_bez, phase); % Not very structured actually ya = q(4:end); y_ = ya-yd; end @@ -119,18 +127,13 @@ q = x(1:n); dq = x(n+1: 2*n); - a_bez = obj.params.legacy.a_bez; - theta_init = obj.params.legacy.theta_init; - theta_end = obj.params.legacy.theta_end; - - dydq_des = obj.dydq_d_gen(q, a_bez, theta_init, theta_end); %[4 x 7] + 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) - % TODO: Not used in RABBIT Lgy = zeros(obj.ydim, 1); end @@ -139,11 +142,7 @@ n = obj.xdim / 2; q = x(1:n); - a_bez = obj.params.legacy.a_bez; - theta_init = obj.params.legacy.theta_init; - theta_end = obj.params.legacy.theta_end; - - dydq_des = obj.dydq_d_gen(q, a_bez, theta_init, theta_end); %[4 x 7] + 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); @@ -155,17 +154,35 @@ n = obj.xdim/2; q = x(1:n); dq = x(n+1:2*n); - - a_bez = obj.params.legacy.a_bez; - theta_init = obj.params.legacy.theta_init; - theta_end = obj.params.legacy.theta_end; - - L2ydq_des = obj.L2ydq_d_gen(q, dq, a_bez, theta_init, theta_end); + + 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/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/get_dynamics_matrices_in_reduced_relative_coord.m b/dynsys/@RabbitBuiltIn/get_dynamics_matrices_in_reduced_relative_coord.m new file mode 100644 index 0000000..f459ce1 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/get_dynamics_matrices_in_reduced_relative_coord.m @@ -0,0 +1,89 @@ +function [D, C, G, B] = get_dynamics_matrices_in_reduced_relative_coord(obj, x) + scale = obj.params.scale; + + %% Parameters + % 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; + + 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); + + % D matrix + D=zeros(5); + D(1,1)=M4*L3^2+M1*L3^2-2*MZ3*L3+2*M3*L3^2+XX3; + D(1,2)=-L3*cos(q32-q31)*(M4*L3+MZ3); + D(1,3)=L4*cos(-q31+q41)*(-MZ3+2*M3*L3+M1*L3+M4*L3); + D(1,4)=-MZ4*L3*cos(-q42+q31); + D(1,5)=-L3*(MY1*sin(q1-q31)+MZ1*cos(q1-q31)); + D(2,1)=-L3*cos(q32-q31)*(M4*L3+MZ3); + D(2,2)=M4*L3^2+XX3; + D(2,3)=-L4*cos(-q32+q41)*(M4*L3+MZ3); + D(2,4)=MZ4*L3*cos(-q42+q32); + D(3,1)=L4*cos(-q31+q41)*(-MZ3+2*M3*L3+M1*L3+M4*L3); + D(3,2)=-L4*cos(-q32+q41)*(M4*L3+MZ3); + D(3,3)=-2*MZ4*L4+2*M4*L4^2+M1*L4^2+2*M3*L4^2+XX4; + D(3,4)=-MZ4*L4*cos(-q42+q41); + D(3,5)=-L4*(MZ1*cos(q1-q41)+MY1*sin(q1-q41)); + D(4,1)=-MZ4*L3*cos(-q42+q31); + D(4,2)=MZ4*L3*cos(-q42+q32); + D(4,3)=-MZ4*L4*cos(-q42+q41); + D(4,4)=XX4; + D(5,1)=-L3*(MY1*sin(q1-q31)+MZ1*cos(q1-q31)); + D(5,3)=-L4*(MZ1*cos(q1-q41)+MY1*sin(q1-q41)); + D(5,5)=XX1; + + % C matrix + C=zeros(5); + C(1,2)=L3*sin(q32-q31)*(M4*L3+MZ3)*dq32; + C(1,3)=-L4*sin(-q31+q41)*(-MZ3+2*M3*L3+M1*L3+M4*L3)*dq41; + C(1,4)=-MZ4*L3*sin(-q42+q31)*dq42; + C(1,5)=-L3*(MY1*cos(q1-q31)-MZ1*sin(q1-q31))*dq1; + C(2,1)=-L3*sin(q32-q31)*(M4*L3+MZ3)*dq31; + C(2,3)=L4*sin(-q32+q41)*(M4*L3+MZ3)*dq41; + C(2,4)=MZ4*L3*sin(-q42+q32)*dq42; + C(3,1)=L4*sin(-q31+q41)*(-MZ3+2*M3*L3+M1*L3+M4*L3)*dq31; + C(3,2)=-L4*sin(-q32+q41)*(M4*L3+MZ3)*dq32; + C(3,4)=-MZ4*L4*sin(-q42+q41)*dq42; + C(3,5)=-L4*(-MZ1*sin(q1-q41)+MY1*cos(q1-q41))*dq1; + C(4,1)=MZ4*L3*sin(-q42+q31)*dq31; + C(4,2)=-MZ4*L3*sin(-q42+q32)*dq32; + C(4,3)=MZ4*L4*sin(-q42+q41)*dq41; + C(5,1)=L3*(MY1*cos(q1-q31)-MZ1*sin(q1-q31))*dq31; + C(5,3)=L4*(-MZ1*sin(q1-q41)+MY1*cos(q1-q41))*dq41; + + % G matrix + G=zeros(5,1); + G(1)=g*sin(q31)*(L3*M1+2*L3*M3-MZ3+L3*M4); + G(2)=-g*sin(q32)*(MZ3+L3*M4); + G(3)=g*sin(q41)*(L4*M1+2*L4*M3+2*L4*M4-MZ4); + G(4)=-g*MZ4*sin(q42); + G(5)=-g*(sin(q1)*MZ1-cos(q1)*MY1); + + % B matrix + B=zeros(5,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(5,1)=-1; + B(5,2)=-1; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/get_hs.m b/dynsys/@RabbitBuiltIn/get_hs.m new file mode 100644 index 0000000..d5e7843 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/get_hs.m @@ -0,0 +1,34 @@ +function hs = get_hs(obj, x) +%% Get the two distance functions (to the circular boundary of the trajectory) +% that are used to define the CBFs. + +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; + +lf=L3*sin(q(1))+L4*sin(q(3))-L3*sin(q(2))-L4*sin(q(4)); + +hf=-L3*cos(q(1))-L4*cos(q(3))+L3*cos(q(2))+L4*cq4; + +h1=R1+obj.step_max-sqrt(hf^2+(R1+lf)^2); + +h2=sqrt((R2+hf)^2+(lf-l_min)^2)-sqrt(R2^2+l_min^2); + +hs = [h1;h2]; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/lf_cbf.m b/dynsys/@RabbitBuiltIn/lf_cbf.m new file mode 100644 index 0000000..81616f1 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/lf_cbf.m @@ -0,0 +1,58 @@ +function LfBs = lf_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; + +[D, C, G, B] = obj.get_dynamics_matrices_in_reduced_relative_coord(x); +inv_D = inv(D); + +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); +N_d2lf=-L3*dq(1)^2*sin(q(1))-L4*dq(3)^2*sin(q(3))+L3*dq(2)^2*sin(q(2))+L4*dq(4)^2*sin(q(4)); +M_d2lf=[L3*cos(q(1)) -L3*cos(q(2)) L4*cos(q(3)) -L4*cos(q(4)) 0]; +L2f_lf=N_d2lf-M_d2lf*inv_D*(C*dq+G); + +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); +N_d2hf=L3*dq(1)^2*cos(q(1))+L4*dq(3)^2*cos(q(3))-L3*dq(2)^2*cos(q(2))-L4*dq(4)^2*cos(q(4)); +M_d2hf=[L3*sin(q(1)) -L3*sin(q(2)) L4*sin(q(3)) -L4*sin(q(4)) 0]; +L2f_hf=N_d2hf-M_d2hf*inv_D*(C*dq+G); + +dh1=-(hf*dhf+(R1+lf)*dlf)/sqrt(hf^2+(R1+lf)^2); +N_d2h1=(hf^2+(R1+lf)^2)^(-3/2)*(hf*dhf+(R1+lf)*dlf)^2-(hf^2+(R1+lf)^2)^(-1/2)*(dhf^2+dlf^2); +M_d2h1=-(hf^2+(R1+lf)^2)^(-1/2); +L2f_h1=N_d2h1+M_d2h1*(hf*L2f_hf+(R1+lf)*L2f_lf); + + +dh2=((R2+hf)*dhf+(lf-l_min)*dlf)/sqrt((R2+hf)^2+(lf-l_min)^2); +N_d2h2=-((R2+hf)^2+(lf-l_min)^2)^(-3/2)*((R2+hf)*dhf+(lf-l_min)*dlf)^2+((R2+hf)^2+(lf-l_min)^2)^(-1/2)*(dhf^2+dlf^2); +M_d2h2=((R2+hf)^2+(lf-l_min)^2)^(-1/2); +L2f_h2=N_d2h2+M_d2h2*((R2+hf)*L2f_hf+(lf-l_min)*L2f_lf); + +Lf_gb1=obj.gamma_b*dh1+L2f_h1; +Lf_gb2=obj.gamma_b*dh2+L2f_h2; +LfBs=[Lf_gb1; Lf_gb2]; +end \ No newline at end of file diff --git a/dynsys/@RabbitBuiltIn/lg_cbf.m b/dynsys/@RabbitBuiltIn/lg_cbf.m new file mode 100644 index 0000000..be45383 --- /dev/null +++ b/dynsys/@RabbitBuiltIn/lg_cbf.m @@ -0,0 +1,44 @@ +function LgBs = lg_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; + +[D, C, G, B] = obj.get_dynamics_matrices_in_reduced_relative_coord(x); +inv_D = inv(D); + +lf=L3*sin(q(1))+L4*sin(q(3))-L3*sin(q(2))-L4*sin(q(4)); +M_d2lf=[L3*cos(q(1)) -L3*cos(q(2)) L4*cos(q(3)) -L4*cos(q(4)) 0]; +LgLf_lf=M_d2lf*inv_D*B; + +hf=-L3*cos(q(1))-L4*cos(q(3))+L3*cos(q(2))+L4*cos(q(4)); +M_d2hf=[L3*sin(q(1)) -L3*sin(q(2)) L4*sin(q(3)) -L4*sin(q(4)) 0]; +LgLf_hf=M_d2hf*inv_D*B; + +M_d2h1=-(hf^2+(R1+lf)^2)^(-1/2); +LgLf_h1=M_d2h1*(hf*LgLf_hf+(R1+lf)*LgLf_lf); + +M_d2h2=((R2+hf)^2+(lf-l_min)^2)^(-1/2); +LgLf_h2=M_d2h2*((R2+hf)*LgLf_hf+(lf-l_min)*LgLf_lf); + +LgBs=[LgLf_h1; LgLf_h2]; +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/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/init_sys_FL.m b/lib/@CtrlAffineSysFL/init_sys_FL.m index a7040e1..5ccc668 100644 --- a/lib/@CtrlAffineSysFL/init_sys_FL.m +++ b/lib/@CtrlAffineSysFL/init_sys_FL.m @@ -1,7 +1,7 @@ function init_sys_FL(obj, params) %% Functions that initialize dynamic system if ~isfield(params, 'use_phase') - obj.use_phase = 1; + obj.use_phase = 0; else obj.use_phase = params.use_phase; obj.params = rmfield(obj.params, 'use_phase'); @@ -24,6 +24,9 @@ function init_sys_FL(obj, params) 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."); From 5baf467c91b8345c9ab73b2ccc11ba99c2ac8405 Mon Sep 17 00:00:00 2001 From: jason Date: Sun, 6 Feb 2022 01:23:10 -0800 Subject: [PATCH 40/49] add rabbit state history plot. --- demos/rabbit/run_stepping_stone_rabbit.m | 40 ++++- demos/rabbit/vis/plot_rabbit_state_history.m | 173 +++++++++++++++++++ 2 files changed, 207 insertions(+), 6 deletions(-) create mode 100644 demos/rabbit/vis/plot_rabbit_state_history.m diff --git a/demos/rabbit/run_stepping_stone_rabbit.m b/demos/rabbit/run_stepping_stone_rabbit.m index 8a3af40..60f3962 100644 --- a/demos/rabbit/run_stepping_stone_rabbit.m +++ b/demos/rabbit/run_stepping_stone_rabbit.m @@ -104,20 +104,48 @@ 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'; +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 -plot_rabbit_result(xs, ts, us, extras); +result.trajectory = xs; +result.stamps = ts; +result.controls = us; +result.forces = extras.forces; +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 @@ -125,6 +153,6 @@ animation_scale = plant_sys.params.scale; global SimConfig SimConfig.m_load=0; -animation_dt = 0.05; -s_quan_vec = coordinateTransformation(xs); -anim_moving_stone_load(ts, s_quan_vec, animation_dt, l_min_t_anim, l_max_t_anim); \ No newline at end of file +% 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 From a7eec076a4762fe3fe9dbb1a2f6e3593437b158b Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sun, 13 Feb 2022 16:48:33 +0900 Subject: [PATCH 41/49] Demurcated the cbf oriented params Signed-off-by: goldenhazard --- demos/rabbit/run_clf_simulation_rabbit.m | 4 ++-- demos/rabbit/run_stepping_stone_rabbit.m | 1 + demos/run_cbf_clf_simulation_2DDI.m | 2 +- demos/run_cbf_clf_simulation_dubins_car.m | 4 ++-- dynsys/@RabbitBuiltIn/RabbitBuiltIn.m | 7 ++++++- 5 files changed, 12 insertions(+), 6 deletions(-) diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index 1b553b7..a4a907d 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -63,7 +63,7 @@ init_clf_simulation_rabbit; dt = 0.025; -%dt = 0.002; +% dt = 0.002; nstep = 10; with_slack = true; verbose_level = 1; @@ -73,7 +73,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 1.0; +plant_sys.params.scale = 1.7; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... diff --git a/demos/rabbit/run_stepping_stone_rabbit.m b/demos/rabbit/run_stepping_stone_rabbit.m index 60f3962..88957a8 100644 --- a/demos/rabbit/run_stepping_stone_rabbit.m +++ b/demos/rabbit/run_stepping_stone_rabbit.m @@ -144,6 +144,7 @@ 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); diff --git a/demos/run_cbf_clf_simulation_2DDI.m b/demos/run_cbf_clf_simulation_2DDI.m index 6e484b5..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; diff --git a/demos/run_cbf_clf_simulation_dubins_car.m b/demos/run_cbf_clf_simulation_dubins_car.m index fc5e9b5..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]; @@ -33,7 +33,7 @@ 'weight_slack', [params.weight_slack, weight_slack_for_cbfs], varargin{:}); [xs, us, ts, extraout] = rollout_controller( ... - x0, dubins, dubins, controller, sim_t); + x0, dubins, dubins, controller, sim_t, 'dt', dt); Bs = extraout.Bs; plot_results(ts, xs, us, Bs, [params.xo;params.yo], params.d) diff --git a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m index d40fa18..648aed6 100644 --- a/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m +++ b/dynsys/@RabbitBuiltIn/RabbitBuiltIn.m @@ -32,7 +32,12 @@ obj.theta_init = obj.params.theta_init; obj.theta_end = obj.params.theta_end; obj.a_bez = obj.params.a_bez; - obj.gamma_b = obj.params.gamma_b; + 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') From 20934399ff5dd353089c9d4a28b46dc58d52cd8f Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 14 Feb 2022 16:38:42 -0800 Subject: [PATCH 42/49] add document. --- docs/Makefile | 20 ++++++++++++++ docs/conf.py | 61 +++++++++++++++++++++++++++++++++++++++++++ docs/index.rst | 47 +++++++++++++++++++++++++++++++++ docs/make.bat | 35 +++++++++++++++++++++++++ docs/requirements.txt | 2 ++ 5 files changed, 165 insertions(+) create mode 100644 docs/Makefile create mode 100644 docs/conf.py create mode 100644 docs/index.rst create mode 100644 docs/make.bat create mode 100644 docs/requirements.txt 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/conf.py b/docs/conf.py new file mode 100644 index 0000000..b8c0d0d --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,61 @@ +# 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 +from pathlib import Path +# 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', + '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' \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000..70af384 --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,47 @@ +.. 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. + +.. toctree:: + :maxdepth: 2 + :caption: Contents: + + +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. + +testing +------- +.. mat:automodule:: lib.@CtrlAffineSys +.. mat:autoclass:: CtrlAffineSys + +.. Links +.. _Symbolic Math Toolbox: https://www.mathworks.com/products/symbolic.html +.. _Manual: https://github.com/HybridRobotics/CBF-CLF-Helper/blob/master/Manual_v1.pdf \ No newline at end of file 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..ea5fa3a --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1,2 @@ +sphinx +sphinxcontrib-matlabdomain \ No newline at end of file From 61316da24b536a0b759e609cdb3991e6f030a44a Mon Sep 17 00:00:00 2001 From: Jason Choi Date: Mon, 14 Feb 2022 16:45:18 -0800 Subject: [PATCH 43/49] Update index.rst --- docs/index.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/index.rst b/docs/index.rst index 70af384..3dc8876 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -41,7 +41,8 @@ testing ------- .. mat:automodule:: lib.@CtrlAffineSys .. mat:autoclass:: CtrlAffineSys +.. mat:autoattribute:: u_max .. Links .. _Symbolic Math Toolbox: https://www.mathworks.com/products/symbolic.html -.. _Manual: https://github.com/HybridRobotics/CBF-CLF-Helper/blob/master/Manual_v1.pdf \ No newline at end of file +.. _Manual: https://github.com/HybridRobotics/CBF-CLF-Helper/blob/master/Manual_v1.pdf From a91d26294d3220ece400593cbf1b7ef2c72d9f49 Mon Sep 17 00:00:00 2001 From: Jay Monga Date: Mon, 14 Feb 2022 16:55:51 -0800 Subject: [PATCH 44/49] Add documentation --- .gitignore | 2 ++ docs/index.rst | 2 +- docs/requirements.txt | 1 + lib/sim/rollout_controller.m | 2 +- 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 033c19e..f5da69e 100644 --- a/.gitignore +++ b/.gitignore @@ -48,3 +48,5 @@ $RECYCLE.BIN/ Network Trash Folder Temporary Items .apdisk + +docs/_build/ \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst index 3dc8876..a5daa99 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -41,7 +41,7 @@ testing ------- .. mat:automodule:: lib.@CtrlAffineSys .. mat:autoclass:: CtrlAffineSys -.. mat:autoattribute:: u_max +.. mat:autoattribute:: CtrlAffineSys.u_max .. Links .. _Symbolic Math Toolbox: https://www.mathworks.com/products/symbolic.html diff --git a/docs/requirements.txt b/docs/requirements.txt index ea5fa3a..4edf719 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,2 +1,3 @@ sphinx +sphinx_rtd_theme sphinxcontrib-matlabdomain \ No newline at end of file diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 5adbee3..873a4b8 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -24,7 +24,7 @@ % 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 +% examples: ctrlClf, ctrlCbfQp, ctrlCbfClfQp % T: simulation time (time horizon) %% Supported fields for varargin % SIMULATION OPTIONS: From 48580d4f895dd89a6d11e3d099c801c4adec685e Mon Sep 17 00:00:00 2001 From: Jay Monga Date: Mon, 14 Feb 2022 17:02:03 -0800 Subject: [PATCH 45/49] roll back accidental change --- lib/sim/rollout_controller.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/sim/rollout_controller.m b/lib/sim/rollout_controller.m index 873a4b8..5adbee3 100644 --- a/lib/sim/rollout_controller.m +++ b/lib/sim/rollout_controller.m @@ -24,7 +24,7 @@ % plant_sys: ControlAffineSys instance for the plant simulation. % control_sys: ControlAffineSys instance for the model and the controller. % controller: function handle for controller. -% examples: ctrlClf, ctrlCbfQp, ctrlCbfClfQp +% examples: ctrlClfQp, ctrlCbfQp, ctrlCbfClfQp % T: simulation time (time horizon) %% Supported fields for varargin % SIMULATION OPTIONS: From ee124a7a05a3b1b873de40de3da9567a80d01cdf Mon Sep 17 00:00:00 2001 From: Jay Monga Date: Mon, 28 Feb 2022 12:50:45 -0800 Subject: [PATCH 46/49] update docs --- .gitignore | 163 +++++++++++++++++- docs/api/classes.rst | 10 ++ docs/api/classes/ctrlaffinesys.rst | 6 + docs/api/classes/ctrlaffinesysfl.rst | 6 + docs/api/functions/sim.rst | 7 + docs/conf.py | 24 ++- docs/github_linkcode.py | 244 +++++++++++++++++++++++++++ docs/index.rst | 17 +- lib/@CtrlAffineSys/CtrlAffineSys.m | 129 +++++++------- 9 files changed, 536 insertions(+), 70 deletions(-) create mode 100644 docs/api/classes.rst create mode 100644 docs/api/classes/ctrlaffinesys.rst create mode 100644 docs/api/classes/ctrlaffinesysfl.rst create mode 100644 docs/api/functions/sim.rst create mode 100644 docs/github_linkcode.py diff --git a/.gitignore b/.gitignore index f5da69e..5f880a7 100644 --- a/.gitignore +++ b/.gitignore @@ -49,4 +49,165 @@ Network Trash Folder Temporary Items .apdisk -docs/_build/ \ No newline at end of file +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/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 index b8c0d0d..3ea5480 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -11,7 +11,10 @@ # 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('../ @@ -31,6 +34,8 @@ # ones. extensions = [ 'sphinx.ext.autodoc', + 'sphinx.ext.napoleon', + 'sphinx.ext.linkcode', 'sphinxcontrib.matlab' ] @@ -58,4 +63,21 @@ # MATLAB matlab_keep_package_prefix = False matlab_src_dir = os.path.dirname(os.path.abspath('.')) -primary_domain = 'mat' \ No newline at end of file +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 index a5daa99..f4a6449 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -3,10 +3,6 @@ You can adapt this file completely to your liking, but it should at least contain the root `toctree` directive. -.. toctree:: - :maxdepth: 2 - :caption: Contents: - Welcome to CBF-CLF-Helper's documentation! ========================================== @@ -37,12 +33,13 @@ Demos ----- Run files in the directory ``demos`` in MATLAB. -testing -------- -.. mat:automodule:: lib.@CtrlAffineSys -.. mat:autoclass:: CtrlAffineSys -.. mat:autoattribute:: CtrlAffineSys.u_max - .. 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/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 2fc8165..857ba8d 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -1,37 +1,33 @@ %% 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 - sdim - % 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_slack + %% 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.) @@ -39,36 +35,43 @@ %% 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') - % 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 - % Optional fields: - % u_min: control min bound (scalar, or the vector of size u_min) - % u_max: control max bound + % | 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 @@ -85,19 +88,29 @@ 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 From 0173cb3e6b284d8ed2245abcb51a75a514c72882 Mon Sep 17 00:00:00 2001 From: goldenhazard Date: Sun, 27 Mar 2022 15:12:41 +0900 Subject: [PATCH 47/49] Kinematic Vehicle Added Signed-off-by: goldenhazard --- demos/rabbit/run_clf_simulation_rabbit.m | 65 ++++++++++++++++++++- demos/rabbit/run_stepping_stone_rabbit.m | 2 +- dynsys/@AccBuiltIn/AccBuiltIn.m | 3 +- dynsys/@AccSymbolic/defineCbf.m | 3 +- dynsys/@KinematicVehicle/KinematicVehicle.m | 49 ++++++++++++++++ lib/@CtrlAffineSys/CtrlAffineSys.m | 60 +++++++++++++++++++ 6 files changed, 178 insertions(+), 4 deletions(-) create mode 100644 dynsys/@KinematicVehicle/KinematicVehicle.m diff --git a/demos/rabbit/run_clf_simulation_rabbit.m b/demos/rabbit/run_clf_simulation_rabbit.m index a4a907d..9b3bf85 100644 --- a/demos/rabbit/run_clf_simulation_rabbit.m +++ b/demos/rabbit/run_clf_simulation_rabbit.m @@ -73,7 +73,7 @@ plant_sys = RabbitBuiltIn(params); % Reflect model uncertainty here -plant_sys.params.scale = 1.7; +plant_sys.params.scale = 1.2; %plant_sys.params.torso_add = 10; clf_qp_controller = @(x, varargin) control_sys... @@ -98,6 +98,69 @@ 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); diff --git a/demos/rabbit/run_stepping_stone_rabbit.m b/demos/rabbit/run_stepping_stone_rabbit.m index 88957a8..59c5542 100644 --- a/demos/rabbit/run_stepping_stone_rabbit.m +++ b/demos/rabbit/run_stepping_stone_rabbit.m @@ -61,7 +61,7 @@ % 0: use slack variable while solving QP % 1: not use slack variable while solving QP -init_clf_simulation_rabbit; +params = init_clf_simulation_rabbit; %% Control Barrier Function Parameters params.cbf.rate = 50; % Used in the cbf. 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/@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/lib/@CtrlAffineSys/CtrlAffineSys.m b/lib/@CtrlAffineSys/CtrlAffineSys.m index 857ba8d..8dbe320 100644 --- a/lib/@CtrlAffineSys/CtrlAffineSys.m +++ b/lib/@CtrlAffineSys/CtrlAffineSys.m @@ -283,6 +283,66 @@ 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 From eded84aa8e5cf7296688c110b7a0e8d5a6be7f44 Mon Sep 17 00:00:00 2001 From: Fernando Date: Thu, 28 Apr 2022 16:09:17 -0700 Subject: [PATCH 48/49] Added cartpole Quanser example. Corrected bug in init_sys --- .../run_cbf_clf_simulation_quanser_cartpole.m | 76 ++++++++++ dynsys/@QuanserCartPole/QuanserCartPole.m | 137 ++++++++++++++++++ lib/@CtrlAffineSys/init_sys.m | 6 +- 3 files changed, 217 insertions(+), 2 deletions(-) create mode 100644 demos/run_cbf_clf_simulation_quanser_cartpole.m create mode 100644 dynsys/@QuanserCartPole/QuanserCartPole.m 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/dynsys/@QuanserCartPole/QuanserCartPole.m b/dynsys/@QuanserCartPole/QuanserCartPole.m new file mode 100644 index 0000000..b2afb4e --- /dev/null +++ b/dynsys/@QuanserCartPole/QuanserCartPole.m @@ -0,0 +1,137 @@ +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; + 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/lib/@CtrlAffineSys/init_sys.m b/lib/@CtrlAffineSys/init_sys.m index 9939e15..b86e323 100644 --- a/lib/@CtrlAffineSys/init_sys.m +++ b/lib/@CtrlAffineSys/init_sys.m @@ -20,12 +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}); From 102a20778e24967f39be532b598c4c30ead0e283 Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 2 May 2022 13:54:47 -0700 Subject: [PATCH 49/49] Added 2weights setting --- dynsys/@QuanserCartPole/QuanserCartPole.m | 3 +++ 1 file changed, 3 insertions(+) diff --git a/dynsys/@QuanserCartPole/QuanserCartPole.m b/dynsys/@QuanserCartPole/QuanserCartPole.m index b2afb4e..f91ea85 100644 --- a/dynsys/@QuanserCartPole/QuanserCartPole.m +++ b/dynsys/@QuanserCartPole/QuanserCartPole.m @@ -108,6 +108,9 @@ 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