%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Fit Forces %% Solve for the hip and leg impedances that will match %% approximate total body forces and torque from cockroach data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Jorge Cham % 05/17/2000 % Biomimetics % % ASSUMPTIONS: % - Feet in world are NOT fixed(precalculated from mid point estimates) % - Fixed hips wrt to the body (precalculated from coxa estimate) % % OVERVIEW: % % Vector to solve: [k1 k2 k3 b1 b2 b3 K1 K2 K3 B1 B2 B3 L1 L2 L3 A1 A2 A3] % Assume symmetry % Li and Ai are nominal leg lengths and angles % % Quantity to minimize: Sum of squares of errors between % approximate body forces and torque and computed forces and torque % from model legs over ALL time points. % % ALGORITHM: % % Find the foot positions and activations pattern from kinematic data % % For each frame: % % - Approximate force and torque from acceleration approximations % - Compute resultant force and torque from legs: % - Find where the feet are in kinematic data % - Compute leg length from hips FIXED in body (as opposed to % getting hip locations from kinematic data % - Compute leg velocity (extension velocity) % - Compute leg angle wrt to body % - Compute leg angle rate wrt to body % - Compute resultant force and torque % - Measure Force error for legs % - Square and sum % - Adjust target vector % - repeat % % Assume data is in: % x_com, y_com, z_com, th_com % vel_x_com, vel_y_com, vel_z_com, % acc_x_com, acc_y_com, acc_z_com, % L1_active, L2_active, ... % L1_foot, L2_foot, ... % time disp(' ') disp('FIT FORCES TO KINEMATIC DATA') disp(' ') disp(' ') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% INITIALIZE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % constants num_legs = 6; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% INITIALIZE DATA STRUCTURE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% bounds = struct(... 'upper_x', 1000, ... 'lower_x', -1000, ... 'upper_y', 1000, ... 'lower_y', -1000, ... 'upper_theta', pi, ... 'lower_theta', -pi ... ); % initialize structure, set values to zero data = struct( ... 'time',zeros(length(time),1),... 'x_com',zeros(length(time),1),... 'y_com',zeros(length(time),1),... 'th_com',zeros(length(time),1),... 'vel_x_com',zeros(length(time),1),... 'vel_y_com',zeros(length(time),1),... 'vel_th_com',zeros(length(time),1),... 'acc_x_com',zeros(length(time),1),... 'acc_y_com',zeros(length(time),1),... 'acc_th_com',zeros(length(time),1),... 'L1_active',zeros(length(time),1),... 'L2_active',zeros(length(time),1),... 'L3_active',zeros(length(time),1),... 'R1_active',zeros(length(time),1),... 'R2_active',zeros(length(time),1),... 'R3_active',zeros(length(time),1),... 'L1_foot',zeros(length(time),3),... 'L2_foot',zeros(length(time),3),... 'L3_foot',zeros(length(time),3),... 'R1_foot',zeros(length(time),3),... 'R2_foot',zeros(length(time),3),... 'R3_foot',zeros(length(time),3),... 'r_hip_com_b', zeros(2,num_legs),... 'mass', zeros(1,1),... 'inertia', zeros(1,1),... 'error_weights', zeros(1,3)... ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% BODY PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %DATA FROM KUBOW AND FULL (1999) data.mass = .0025; % Kg data.inertia = 2.04e-7; % Kg m^2 data.time = time; data.x_com = x_com; data.y_com = y_com; data.th_com = th_com; data.vel_x_com = vel_x_com; data.vel_y_com = vel_y_com; data.vel_th_com = vel_th_com; data.acc_x_com = acc_x_com; data.acc_y_com = acc_y_com; data.acc_th_com = acc_th_com; data.L1_active = L1_active; data.L2_active = L2_active; data.L3_active = L3_active; data.R1_active = R1_active; data.R2_active = R2_active; data.R3_active = R3_active; data.L1_foot = L1_foot; data.L2_foot = L2_foot; data.L3_foot = L3_foot; data.R1_foot = R1_foot; data.R2_foot = R2_foot; data.R3_foot = R3_foot; % hip locations from movie of 3377 data.r_hip_com_b(:,1) = [-0.00225 .015]'; data.r_hip_com_b(:,2) = [-0.0035 0.011]'; data.r_hip_com_b(:,3) = [-0.00425 0.004]'; data.r_hip_com_b(:,4) = [0.00225 .015]'; data.r_hip_com_b(:,5) = [0.0035 0.011]'; data.r_hip_com_b(:,6) = [0.00425 0.004]'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% SEARCH PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Make initial guess based on range of forces observed in Kubow and Full % In general, forces are in the 0.007 (0.005 vertical and horizontal) range. % Hind legs are a bit higher (0.010). % From this, assume that forces are half due to damping and velocity % Approximation of leg compression from movies is about 10mm = .010 m % Velocity approximation is about 100mm/s = 0.1m/s % k = f/x = 0.004/0.010 = 0.4 % k = b/v = 0.004/0.1 = 0.04 % % Assume same for rotational elements % K = T/TH = (0.004*0.01)/(pi/2) = 2.5e-5 % B = T/TH_dot = (0.004*0.01)/2 = 2.0e-5 data.error_weights = [1 1 100]; k = 0.4; b = 0.04; K = 2.5e-5; B = 2.0e-6; L1 = 0.010; L2 = 0.015; L3 = 0.025; A1 = 150*pi/180; A2 = 170*pi/180; A3 = 260*pi/180; %A4 = 30*pi/180; %A5 = 10*pi/180; %A6 = -80*pi/180; % Vector to solve: [k1 k2 ... b1 b2 ... K1 K2 ... B1 B2 ... L1 L2 ... A1 A2 ...] % 36 elements! x0 = [k k k b b b K K K B B B L1 L2 L3 A1 A2 A3] options = optimset('MaxIter',100); %placeholder for fminunc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% FIGURES PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% movie_index = 1; resolution = '-r50'; make_frames = 0; xmax = .075; xmin = -.075; ymax = .15; ymin = -.02; bodycolor = 'b'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% START SEARCH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %X = FMINUNC('optimfunc_1',x0,options, data) [X f] = fminsearch('optimfunc_1',x0,options, data)