Merge branch 'main' of https://github.com/ChenZhenY/sl_hopping

Optimization/hopping_constraints.asv

@@ -0,0 +1,74 @@
+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 = x(1);
+    ctrlpts = reshape(x(2:end),[2],[]);
+    ctrlpts = vertcat(ctrlpts, zeros(1,size(ctrlpts,2)));
+    [tout, zout, uout, indices, slip_out] = hybrid_simulation(z0, ctrlpts, p, [0 tf],1);
+    theta1 = zout(1,:); 
+    theta2 = zout(2, :);
+    COM = COM_jumping_leg(zout,p);
+    ground_height = p(end-1);
+    pos_leg = position_foot(zout(:,end),p);
+
+    pend_vector_begin = position_foot(zout(:,1),p) - position_mount(zout(:,1),p);
+    pend_vector_end = position_foot(zout(:,end),p) - position_mount(zout(:,end),p);
+
+    % find angles to the ground
+    u = [1; 0; 0];
+    cos_theta = max(min(dot(u,pend_vector_begin)/(norm(u)*norm(pend_vector_begin)),1),-1);
+    touchdown_angle = real(acos(cos_theta));
+    cos_theta = max(min(dot(u,pend_vector_begin)/(norm(u)*norm(pend_vector_begin)),1),-1);
+    liftoff_angle = real(acos(cos_theta));
+
+    % 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 = [...
+        touchdown_angle + liftoff_angle - pi...
+        ]; % y 
+
+    %ceq = [pos_leg(2)]; % y 
+end

Optimization/hopping_constraints.m

@@ -43,18 +43,22 @@
         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
-
+
+    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 = [...
+%         touchdown_angle + liftoff_angle - pi...
+        ]; % y 
+
     %ceq = [pos_leg(2)]; % y 
     ceq = [];
 end

Optimization/hopping_objective.asv

@@ -0,0 +1,50 @@
+function f = hopping_objective(x,z0,p)
+% Inputs:
+% x - an array of decision variables.
+% z0 - the initial state
+% p - simulation parameters
+% 
+% Outputs:
+% f - scalar value of the function (to be minimized) evaluated for the
+%     provided values of the decision variables.
+%
+% Note: fmincon() requires a handle to an objective function that accepts 
+% exactly one input, the decision variables 'x', and returns exactly one 
+% output, the objective function value 'f'.  It is convenient for this 
+% assignment 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().
+
+    % numerically integrate to the final height
+    tf = x(1);
+    ctrlpts = reshape(x(2:end),[2],[]);
+    ctrlpts = vertcat(ctrlpts, zeros(1,size(ctrlpts,2)));
+    [tout, zout, uout, indices, slip_out] = hybrid_simulation(z0, ctrlpts, p, [0 tf],1);
+    COM = COM_jumping_leg(zout,p);
+    % f = -COM_end;                                           % negative of COM height
+     f = -max(COM(1,:));
+
+    pend_vector_end = position_foot(zout(:,end),p) - position_mount(zout(:,end),p);
+
+    % find angles to the ground
+    v = pend_vector_end / norm(pend_vector_end);
+
+%    t0 = 0; tend = tf;   % set initial and final times
+%    dt = 0.001;
+%    num_step = floor((tend-t0)/dt);
+%     torque = zeros(1, num_step);
+%     for i = 1:num_step-1
+%         torque(i) = BezierCurve(ctrl.T, i*dt/ctrl.tf);
+%     end
+%     work = torque*torque.';
+%     f = work;                                         % minimize T^2 integral
+% f = -(zout(4,end) - zout(4,1));
+%   f = -zout(4,end);  
+    end_vel = zout(end-1:end,end); % get x and y component velocities
+    end_vel = vertcat(end_vel, [0]); % append z
+    f = -dot(v, end_vel); % minimize negative end velocity
+
+>>>>>>> 4db284cad6079348bc2a99697caef2bfe7b40558
+end

Optimization/hopping_objective.m

@@ -37,5 +37,9 @@
 %     work = torque*torque.';
 %     f = work;                                         % minimize T^2 integral
 % f = -(zout(4,end) - zout(4,1));
-  f = -zout(4,end);  
+%   f = -zout(4,end);  
+    end_vel = zout(end-1:end,end); % get x and y component velocities
+    end_vel = vertcat(end_vel, [0]); % append z
+    f = -dot(v, end_vel); % minimize negative end velocity
+
 end

run_simulation.m

@@ -34,18 +34,20 @@
 %__________________________ run hopping leg_________________________________________
 if run_hopping
 
-%     ctrl_rd = rand(2,4)*2;
-    ctrl = [-.5 .5 0 0.0 ; 1 .5 0 0];                     % control values
+  %  ctrl_rd = rand(2,4)*2;
+    ctrl = [1 1 ; 1 1];                     % control values
     %ctrl(1:3,1:3) = 0;
                                             % one row for one motor control points
 % optimization start                                            
     x = ctrl;
     % % setup and solve nonlinear programming problem
     problem.objective = @(x) hopping_objective(x,z0,p);     % create anonymous function that returns objective
     problem.nonlcon = @(x) hopping_constraints(x,z0,p);     % create anonymous function that returns nonlinear constraints
-    problem.x0 = [ctrl];                   % initial guess for decision variables
-    problem.lb = [-2*ones(size(ctrl))];     % lower bound on decision variables
-    problem.ub = [2*ones(size(ctrl))];     % upper bound on decision variables
+    problem.x0 = x;                   % initial guess for decision variables
+    problem.lb = [-2*ones(size(x))];     % lower bound on decision variables
+    problem.lb(1) = 0;
+    problem.ub = [2*ones(size(x))];     % upper bound on decision variables
+    problem.ub(1) = 2; % say max time 
     problem.Aineq = []; problem.bineq = [];         % no linear inequality constraints
     problem.Aeq = []; problem.beq = [];             % no linear equality constraints
     problem.options = optimset('Display','iter');   % set options