Little update

Modify constraints to make it easier to solve for fmincon, save parameters that can properly hop vertically.

Optimization/hopping_constraints.asv

@@ -0,0 +1,59 @@
+function [cineq ceq] = hopping_constraints(x,z0,p)
+% Inputs:
+% x - an array of decision variables.
+% z0 - the initial state
+% p - simulation parameters
+% 
+% Outputs:
+% cineq - an array of values of nonlinear inequality constraint functions.  
+%         The constraints are satisfied when these values are less than zero.
+% ceq   - an array of values of nonlinear equality constraint functions.
+%         The constraints are satisfied when these values are equal to zero.
+%
+% Note: fmincon() requires a handle to an constraint function that accepts 
+% exactly one input, the decision variables 'x', and returns exactly two 
+% outputs, the values of the inequality constraint functions 'cineq' and
+% the values of the equality constraint functions 'ceq'. It is convenient 
+% in this case to write an objective function which also accepts z0 and p 
+% (because they will be needed to evaluate the objective function).  
+% However, fmincon() will only pass in x; z0 and p will have to be
+% provided using an anonymous function, just as we use anonymous
+% functions with ode45().
+
+% todo: add slip constraints
+
+    tf = p(end);
+    ctrl = x;
+    [tout, zout, uout, indices, slip_out] = hybrid_simulation(z0, ctrl, p, [0 tf]);
+    theta1 = zout(1,:); 
+    theta2 = zout(2, :);
+    COM = COM_jumping_leg(zout,p);
+    ground_height = p(end-1);
+    pos_leg = position_leg(zout(:,end),p)
+
+    % check if anything other than the hopping foot touches the ground
+
+    sw_Cy = zeros(numel(tout),1); k_Cy = zeros(numel(tout),1); h_Cy = zeros(numel(tout),1);
+    for i = 1:numel(tout)
+        sw_pos = position_swinging_foot(zout(:, i),p);
+        sw_Cy(i) = sw_pos(2) - ground_height;      % foot height from ground
+        k_pos = position_knee(zout(:, i),p);
+        k_Cy(i) = k_pos(2) - ground_height;        % knee height from ground
+        h_pos = position_hip(zout(:, i),p);
+        h_Cy(i) = h_pos(2) - ground_height;        % hip height from ground
+    end
+
+    cineq = [-theta2(:) + pi/6;... % joint limits
+        theta1(:) + theta2(:)-2*pi/3;...
+        theta2(:) - 2*pi/3;...
+        -theta1(:) - pi/3;...
+        -sw_Cy(:); ...            % swinging leg does not contact floor
+        -k_Cy(:); ...             % knee does not contact floor
+        -h_Cy(:);...              % hip does not contact floor
+        slip_out(:);...           % foot does not slip
+        -(zout(4,end) - zout(4,1) - .1) ]; % leg moves forward in x direction
+
+    % ceq = [min(zout(3,:)) - max(zout(3,:))]; % swing leg angle stays fixed
+    ceq = [zout()];
+
+end

Optimization/hopping_constraints.m

@@ -29,6 +29,7 @@
     theta2 = zout(2, :);
     COM = COM_jumping_leg(zout,p);
     ground_height = p(end-1);
+    pos_leg = position_foot(zout(:,end),p);
 
     % check if anything other than the hopping foot touches the ground
 
@@ -42,16 +43,17 @@
         h_Cy(i) = h_pos(2) - ground_height;        % hip height from ground
     end
 
-    cineq = [-min(theta2) + pi/6;... % joint limits
-        max(theta1 + theta2)-2*pi/3;...
-        max(theta2) - 2*pi/3;...
-        -min(theta1) - pi/3;...
-        -min(sw_Cy); ...            % swinging leg does not contact floor
-        -min(k_Cy); ...             % knee does not contact floor
-        -min(h_Cy);...              % hip does not contact floor
-        max(slip_out);...           % foot does not slip
+    cineq = [-theta2(:) + pi/6;... % joint limits
+        theta1(:) + theta2(:)-2*pi/3;...
+        theta2(:) - 2*pi/3;...
+        -theta1(:) - pi/3;...
+        -sw_Cy(:); ...            % swinging leg does not contact floor
+        -k_Cy(:); ...             % knee does not contact floor
+        -h_Cy(:);...              % hip does not contact floor
+        slip_out(:);...           % foot does not slip
         -(zout(4,end) - zout(4,1) - .1) ]; % leg moves forward in x direction
 
-    ceq = [min(zout(3,:)) - max(zout(3,:))]; % swing leg angle stays fixed
+    % ceq = [min(zout(3,:)) - max(zout(3,:))]; % swing leg angle stays fixed
+    ceq = [pos_leg(2)]; % y 
 
 end

Optimization/hopping_objective.m

@@ -35,6 +35,6 @@
 %     end
 %     work = torque*torque.';
 %     f = work;                                         % minimize T^2 integral
-f = -(zout(5,end) - zout(5,1));
-
+% f = -(zout(4,end) - zout(4,1));
+  f = -zout(5,end);  
 end

Visualization/animate.m

@@ -12,7 +12,7 @@ function animate(tspan, x, p)
     h_title = title('t=0.0s');
 
     axis equal
-    x_min = -.2;
+    x_min = -.8;
     x_max = .2;
     axis([x_min x_max -.3 .1]*3); % -.2 .2 -.3 .1
     height = p(end-1);

run_simulation.m

@@ -16,20 +16,21 @@
 % set to run optimization over hopping leg or over swing leg
 run_hopping = true;
 
-z0 = [0; pi/4; -pi/3; 0; 0; 0; 0; 0; 0; 0];    
+z0 = [-pi/3; pi/3; 0; 0; 0; 0; 0; 0; 0; 0];    
 p = parameters();                           % get parameters from file
 pos_foot0 = position_foot(z0, p);  
 ground_height = pos_foot0(2);
-tf = 1.5;                                   %simulation time
+tf = 0.6;                                   %simulation time
 p = [p; ground_height; tf];
 
 % An equation has been added to dynamics_continuous and dynamics_discrete
 % to integrate this new state.
 
 %__________________________ run hopping leg_________________________________________
 if run_hopping
-
-    ctrl = ones(3,6)*0.7;                     % control values
+
+    ctrl_rd = rand(3,4)*2;
+    ctrl = ctrl_rd;                     % control values
     %ctrl(1:3,1:3) = 0;
                                             % one row for one motor control points
 % optimization start                                            

test.m

@@ -0,0 +1,9 @@
+
+for i = 1:numel(tout)
+    sw_pos = position_swinging_foot(zout(:, i),p);
+    sw_Cy(i) = sw_pos(2) - ground_height;      % foot height from ground
+    k_pos = position_knee(zout(:, i),p);
+    k_Cy(i) = k_pos(2) - ground_height;        % knee height from ground
+    h_pos = position_hip(zout(:, i),p);
+    h_Cy(i) = h_pos(2) - ground_height;        % hip height from ground
+end