%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Optim Function %% Returns the sum of squared errors to force estimates from kinematic %% data. %% %% %% %% data = struct( ... %% '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)... %% );%% %% %% %% 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 %% %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [xreturn] = optimfunc(X, data) % this function is called as % X=FMINUNC(FUN,X0,OPTIONS,data) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% INITIALIZE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mass = data.mass; inertia = data.inertia; time = data.time; num_legs = 6; %% Define Leg Properties as function of leg_index: %% [k1 k2 k3 b1 b2 b3 K1 K2 K3 B1 B2 B3 L1 L2 L3 A1 A2 A3] X_k = [X(1:3) X(1:3)]; X_b = [X(4:6) X(4:6)]; X_K = [X(7:9) X(7:9)]; X_B = [X(10:12) X(10:12)]; X_nominal_length = [X(13:15) X(13:15)]; %% Leg angles are not symmetric, so subtract 180 degrees X_nominal_angle = [X(16:18) pi-X(16) pi-X(17) pi-X(18)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% FRAME FOR LOOP %% For each leg, compute the force contribution %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% initialize frame_force_error_sum = [0;0]; frame_moment_error_sum = 0; for iF=1:length(time), %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Compute data force and torque data_force_x = data.acc_x_com(iF)*mass; data_force_y = data.acc_y_com(iF)*mass; data_force_th = data.acc_th_com(iF)*inertia; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% LEG FOR LOOP %initialize loop leg_force_sum = [0;0]; leg_moment_sum = 0; %set up state variables r_com_0_w = [data.x_com(iF); data.y_com(iF)]; r_com_0_dot_w = [data.vel_x_com(iF); data.vel_y_com(iF)]; theta_b_w = data.th_com(iF); theta_b_dot_w = data.vel_th_com(iF); data_r_foot_0_w = [data.L1_foot(iF,1:2)' data.L2_foot(iF,1:2)' data.L3_foot(iF,1:2)' ... data.R1_foot(iF,1:2)' data.R2_foot(iF,1:2)' data.R3_foot(iF,1:2)']; leg_active = [data.L1_active(iF) data.L2_active(iF) data.L3_active(iF)... data.R1_active(iF) data.R2_active(iF) data.R3_active(iF)]; for iL = 1:num_legs if leg_active(iL), %set up leg variables r_foot_0_w = data_r_foot_0_w(:,iL); r_hip_com_b = data.r_hip_com_b(:,iL); leg_length = X_nominal_length(iL); k = X_k(iL); b = X_b(iL); K = X_K(iL); B = X_B(iL); nominal_angle = X_nominal_angle(iL); %rotation matrices wRb = [cos(theta_b_w) -sin(theta_b_w); sin(theta_b_w) cos(theta_b_w)]; wRb_dot = [-theta_b_dot_w*sin(theta_b_w) -theta_b_dot_w*cos(theta_b_w); ... theta_b_dot_w*cos(theta_b_w) -theta_b_dot_w*sin(theta_b_w)]; %define vectors (don't try this at home - or without training) r_hip_foot_w = r_com_0_w + wRb*r_hip_com_b - r_foot_0_w; r_hip_foot_dot_w = r_com_0_dot_w + wRb_dot*r_hip_com_b; % ADDED BY JORGE (THE VECTOR INSIDE THE ATAN2 SHOULD BE NEGATIVE) theta_hip_w = atan2(-r_hip_foot_w'*[0;1], -r_hip_foot_w'*[1;0]); %theta_hip_w = atan2(r_hip_foot_w'*[0;1], r_hip_foot_w'*[1;0]); theta_hip_b = theta_hip_w - theta_b_w; theta_hip_dot_w = r_hip_foot_dot_w'*[0 -1; 1 0]*r_hip_foot_w / (norm(r_hip_foot_w))^2; theta_hip_dot_b = theta_hip_dot_w - theta_b_dot_w; %leg impedance forces leg_spring_force = k * (leg_length - norm(r_hip_foot_w)) * r_hip_foot_w / ... norm(r_hip_foot_w); leg_damping_force = -b * (r_hip_foot_dot_w' * r_hip_foot_w / norm(r_hip_foot_w)) * ... r_hip_foot_w / norm(r_hip_foot_w); %leg impedance moments r_hip_com_w = wRb*r_hip_com_b; cross_product = cross([r_hip_com_w;0], [leg_spring_force;0]); leg_spring_moment = cross_product(3); cross_product = cross([r_hip_com_w;0], [leg_damping_force;0]); leg_damping_moment = cross_product(3); % ADDED BY JORGE % 1 - Signs on the hip spring and damping error terms were wrong sign % (also B is wrong sign, though previous error cancelled it) % 2 - Because atan2 is limited to -pi to pi, we must take measures for % when nominal is on the upper-left quadrant and the actual is on % the lower-left quadrant (or vice-versa) % hip impedance torques, resultant forces, resultant moments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 nom_x = cos(nominal_angle); nom_y = sin(nominal_angle); nominal_angle = atan2(nom_y,nom_x); hip_x = cos(theta_hip_b); hip_y = sin(theta_hip_b); theta_hip_b = atan2(hip_y,hip_x); theta_delta = -nominal_angle + theta_hip_b; if theta_delta>pi, theta_delta = -2*pi+theta_delta; end if theta_delta<-pi, theta_delta = 2*pi+theta_delta; end hip_spring_torque = K * (theta_delta); %hip_spring_torque = K * (-nominal_angle + theta_hip_b) hip_damping_torque = -B * (-theta_hip_dot_b); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %hip_spring_torque = K * (nominal_angle - theta_hip_b); %hip_damping_torque = -B * theta_hip_dot_b; hip_resultant_force = -(hip_spring_torque+hip_damping_torque) / ... norm(r_hip_foot_w) * [0 -1; 1 0] * r_hip_foot_w / norm(r_hip_foot_w); cross_product = cross([r_hip_com_w;0], [hip_resultant_force;0]); hip_resultant_moment = cross_product(3); %add the effects for this leg leg_force = leg_spring_force + leg_damping_force + hip_resultant_force; leg_moment = leg_spring_moment + leg_damping_moment + hip_spring_torque + ... hip_damping_torque + hip_resultant_moment; % IGNORE LIFTOFF OR PULLING CONDITION FOR NOW %check foot condition %force must be upwards acting %if( (leg_force'*[0;1]) < 0 ) % disp(... % strcat('Leg ', ... % strcat(num2str(leg_index), ... % strcat(' has an upward acting force at time ', num2str(time))... % )... % )... % ); % leg_force = [0;0]; % leg_moment = 0; %end %leg must be long enough to reach the floor %if (norm(r_hip_foot_w) > leg_length ) % disp(... % strcat('Leg ', ... % strcat(num2str(leg_index), ... % strcat(' cannot reach the floor at time ', num2str(time))... % )... % )... % ); % leg_force = [0;0]; % leg_moment = 0; %end %add the contribution of this leg to the aggregate leg_force_sum = leg_force_sum + leg_force; leg_moment_sum = leg_moment_sum + leg_moment; end % end of leg_active 'if' statement end % end of leg_index 'for' loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Get errors for this frame frame_force_error = [(data_force_x-leg_force_sum(1))^2; (data_force_y-leg_force_sum(2))^2]; frame_moment_error= (data_force_th - leg_moment_sum)^2; frame_force_error_sum = frame_force_error_sum + frame_force_error; frame_moment_error_sum = frame_moment_error_sum + frame_moment_error; end % frame_index 'for' loop xreturn = frame_force_error_sum(1)*data.error_weights(1) + ... frame_force_error_sum(2)*data.error_weights(2) + ... frame_moment_error_sum*data.error_weights(3);