基于matlab单脚架漏斗.zip

function out_vector = state_control(feedback, command, oldstate) % STATE_CONTROL Modified version of monopod hopper controller % first proposed by Mark Raibert. % out_vector = state_control(feedback, command, oldstate) % inputs : % feedback : The feedback arguments is comprised of feedback % signals coming from the robot and ground blocks. These % feedback signals include, robot joint positions, robot % joint velocities, leg tip position and velocities, the % state of in contact, and finally the ground forces % applied to the leg tip. % command : The command is comprised of two elements. First is % the desired hight and second is the desired speed. % oldstate : % The old state argument includes those states % of the previous time step of the controller which are % necessary for estimating the control signal at the % current time step. These are indeed the internal states % of the controller from the previous time step. % The state used in this m file are : % newstate =[tip_pos; tip_vel; q ; qd ; incontact; % max_hight;f_ext;count_thrust;count_loading; % phi_desired;flag_fp; tau_thrust;power;Kd]; % % Output : % out_vector : Output vector includes the old state signal % which is kept in a delay block for use in the next % time step, % and the commanded torques sent to the actuators % q = feedback(1:5, 1); qd = feedback (6:10 ,1); tip_pos = feedback (11:12 ,1); tip_vel = feedback (13:14 ,1); incontact = feedback (15); f_ext = feedback(16:18); hight_desired = command(1,1); speed_desired = command(2,1); if(oldstate == zeros(size(oldstate))) % newstate =[tip_pos; tip_vel; q ; qd ; incontact; max_hight;f_ext;count_thrust;count_loading;count_stance;stance_duration;tau_thrust]; oldstate = [tip_pos;tip_vel;q;qd;incontact;q(2,1);0;0;0;200;200;1000;1; 0]; %count = []; %count : controls pulse duration for the thrust phase max_hight = oldstate(16,1); end tip_pos_old = oldstate(1:2,1); tip_vel_old = oldstate(3:4,1); q_old = oldstate(5:9,1); qd_old = oldstate(10:14,1); incontact_old = oldstate(15,1); max_hight = oldstate(16,1); f_ext_old = oldstate(17:19); count_thrust = oldstate(20,1); count_loading = oldstate(21,1); phi_desired = oldstate(22,1); flag_fp = oldstate(23,1); % max_hight_old = oldstate(24,1); tau_thrust = oldstate(24,1); d_escape = .02; %determines the tolerance of the robot hight for the scape phase state =0; %reset the state to zero each time step sample_t = .001; loading_pulse_width = 70; %--mili-secs thrust_pulse_width = 100; %mili-secs tau = [0;0;0;0;0]; %----initialize the thrust to zero each time step tau4 = 0; stance_duration = .174; %--------------foot placement algorithm----------------% k_xdot = .01; % gain xf = qd(1,1) * stance_duration /2 + k_xdot * (qd(1,1) - speed_desired); phi_desired = asin(xf/.4) - q(3,1); % desired hip angle for landing (used for foot % placement control) %-------------Body Attitude Setpoint ---------------% phi2_desired = 0; % th5_desired = 0; %------------Leg springiness and damping constant --------------% Kp= 2000; Kd = 0; %----------------- Set the controller gains for 3 objectives :---------% %------------------hight control, foot position control-------------------% %------------------,body attitude control ---------------% Kp_fplacement = 50; Kd_fplacement = 1; Kp_battitude = 200; Kd_battitude = 20; Kp_thrust = 20; Kd_thrust = 1000; %--------------Detect Robot State--------------% if(incontact == 1) if(incontact_old ==0 && count_loading == 200) count_loading = 0; elseif ( f_ext(3,1) > 20) if(tip_vel(2,1) < 0) state = 2; elseif(tip_vel(2,1) > 0) state = 4; end; if(tip_vel(2,1) >= 0 && tip_vel_old(2,1) <= 0) if(count_thrust == 200 && count_loading== loading_pulse_width) count_thrust = 0; end end elseif( f_ext(3,1) < 20 && tip_vel(2,1) > 0) state = 5; end end if(incontact == 0) state = 7; if(tip_pos(2,1)< .1) state = 6; end end %--------loading pulse--------% %---------thrust pulse--------% if(count_loading < loading_pulse_width) state = 1; count_loading = count_loading +1; elseif(count_thrust < thrust_pulse_width) state = 3; count_thrust = count_thrust + 1; end % ----- Force component due to the springy leg with damper ------% % tau = [0;0;0;0;-Kd] .* qd + [0;0;0;0;-Kp] .* q; tau = [0;0;0;0;0]; power = 0; %---------State Machine---------% switch(state) case 1 %--------------loading---------------- %--------------zero hip torque-------- tau4 = 0; tau4 = qd(4,1) * 100 * 0.0235; case 2 %--------------compression------------- %-----Servo body attitude with hip----- tau4 = +Kp_battitude *(q(3,1)-phi2_desired) + Kd_battitude * qd(3,1) ; % tau4 = -min(tau4,30); % tau4 = qd(4,1) * 100 * 0.0235; case 3 %--------------thrust----------------- %-------apply pulse for thrust-------- hight_error = hight_desired - max_hight; % if(count_thrust == 1) % tau_diff = (hight_error) * 300; % tau_thrust = 11.5 + tau_diff; % end % tau_thrust = - q(5) * 2000 - (q(5) - q_old(5))*2000; % tau_thrust = - 350; th5_desired = 10; % tau = tau + [0;0;0;0;-tau_thrust] ; Power = 0; %-------servo body attitude with hip ----------% tau4 = Kp_battitude *(q(3,1)-phi2_desired) + Kd_battitude * qd(3,1) ; % tau4 = -tau4 + qd(4,1) * 100 * 0.0235; % case 4 %------------decompression-------------% % ------------stop thrust % ------------servo body attitude with hip torque tau4 = qd(4,1) * 100 * 0.0235; case 5 %--------------unloading--------------- % ------------no thrust-------------- % ------------no hip torque----------- tau4 = qd(4,1) * 100 * 0.0235; case 6 %--------------escape-------------------- %--------reset count variables---------------% Kd = 200; if(count_thrust <200) %max_hight_old = max_hight; max_hight = 0; % resets max_hight in decompression phase count_thrust = 200; count_loading = 200; % count_stance = 1000; flag_fp = 1; end case 7 %---------------flight------------------- %----recalculate max_hight of the robot each time step---- Kd = 200; if(q(2,1) > max_hight) max_hight = q(2,1); end %-----------Position leg for landing------------ tau4 = -Kp_fplacement *(q(4,1)-phi_desired) - Kd_fplacement * qd(4,1) ; end newstate =[tip_pos; tip_vel; q ; qd ; incontact; max_hight;f_ext;count_thrust;count_loading;phi_desired;flag_fp; th5_desired;power;Kd;state]; out_vector = [[0;0;Kd;tau4;0] ; newstate];