see #7

art3/+opt/GetBounds.m

@@ -105,12 +105,12 @@
 
     % start swing foot velocity, pos vel
     % (Make sure foot goes upward)
-    model_bounds.constrBounds.footVelocityBeginning.lb = [ -5, 0, 0]';
-    model_bounds.constrBounds.footVelocityBeginning.ub = [ 5, 0, 10]';
+    model_bounds.constrBounds.footVelocityBeginning.lb = [ -0.2, 0, 0]';
+    model_bounds.constrBounds.footVelocityBeginning.ub = [ 0.2, 0, 0.3]';
     % impact swing foot velocity, pos vel
     % (Make sure foot goes downward and slightly backward)    
-    model_bounds.constrBounds.footVelocityEnd.lb = [ -5, 0, -10]';
-    model_bounds.constrBounds.footVelocityEnd.ub = [ 5, 0, -0]';
+    model_bounds.constrBounds.footVelocityEnd.lb = [ -0.2, 0, -0.25]';
+    model_bounds.constrBounds.footVelocityEnd.ub = [ 0.2, 0, -0.05]';
 
 
     %%% Common Virtual Constraints
@@ -120,25 +120,34 @@
     model_bounds.params.aposition.ub = 100*ones(6*4,1);
     model_bounds.params.aposition.x0 = zeros(6*4,1);
     % phase paramaters for virtual constraints "position"
-    model_bounds.params.pposition.lb = [model_bounds.time.t0.lb, model_bounds.time.tf.lb]'; 
-    model_bounds.params.pposition.ub = [model_bounds.time.t0.ub, model_bounds.time.tf.ub]';   
-    model_bounds.params.pposition.x0 = [model_bounds.time.t0.x0, model_bounds.time.tf.x0]';
-
+%     model_bounds.params.pposition.lb = [model_bounds.time.t0.lb, model_bounds.time.tf.lb]'; 
+%     model_bounds.params.pposition.ub = [model_bounds.time.t0.ub, model_bounds.time.tf.ub]';   
+%     model_bounds.params.pposition.x0 = [model_bounds.time.t0.x0, model_bounds.time.tf.x0]';
+
+    if vx >= 0
+        model_bounds.params.pposition.lb = [-2*T_*vx, 0*vx]';
+        model_bounds.params.pposition.ub = [T_*vx, 2*T_*vx]';
+        model_bounds.params.pposition.x0 = [-0.5*T_*vx, 0.5*T_*vx]';
+    else
+        model_bounds.params.pposition.ub = [-T_*vx, 0*vx]';
+        model_bounds.params.pposition.lb = [0*vx, T_*vx]';
+        model_bounds.params.pposition.x0 = [-0.5*T_*vx, 0.5*T_*vx]';
+    end
 
     % some trick for foot touchdown position
 
-    left_foot_position_init = vx*T_;
-    left_foot_position_max = vx*T_ + 0.02;
-    left_foot_position_min = vx*T_ - 0.02;
+%     left_foot_position_init = vx*T_;
+%     left_foot_position_max = vx*T_ + 0.02;
+%     left_foot_position_min = vx*T_ - 0.02;
 
 
     model_bounds.params.pRightPoint.lb = [0, -wt/2.0, 0]';    
     model_bounds.params.pRightPoint.ub = [0, -wt/2.0, 0]';    
     model_bounds.params.pRightPoint.x0 = [0, -wt/2.0, 0]';
 
-    model_bounds.params.pLeftPoint.lb = [left_foot_position_min, wt/2.0, 0]';    
-    model_bounds.params.pLeftPoint.ub = [left_foot_position_max, wt/2.0, 0]';     
-    model_bounds.params.pLeftPoint.x0 = [left_foot_position_init, wt/2.0, 0]';
+    model_bounds.params.pLeftPoint.lb = [-0.1, wt/2.0, 0]';    
+    model_bounds.params.pLeftPoint.ub = [1, wt/2.0, 0]';     
+    model_bounds.params.pLeftPoint.x0 = [vx*T_, wt/2.0, 0]';
 
 
     %% construct the boundary values for each domain 

art3/+sys/+domains/LeftStance.m

@@ -57,10 +57,20 @@
     %     p = SymVariable('p',[2,1]);
     %     tau = (deltaPhip-p(2))/(p(1)-p(2));
 
-    % @time based phase variable
-    t = SymVariable('t');
+    l_hip_frame = domain.Joints(getJointIndices(domain,'l_hip_pitch'));
+    p_lhp = getCartesianPosition(domain, l_hip_frame);
+    p_lf = getCartesianPosition(domain, left_foot);
+    p_hip = p_lhp(1) - p_lf(1);
+    deltaPhip = linearize(p_hip,x);
+
+    % @state based phase variable
     p = SymVariable('p',[2,1]);
-    tau = (t-p(1))/(p(2)-p(1));
+    tau = (deltaPhip-p(1))/(p(2)-p(1));
+
+    % @time based phase variable
+%     t = SymVariable('t');
+%     p = SymVariable('p',[2,1]);
+%     tau = (t-p(1))/(p(2)-p(1));
 
 
     % relative degree two outputs:
@@ -81,7 +91,7 @@
         'RightKneePitch'};
 
     y2 = VirtualConstraint(domain,ya_2,'position','DesiredType','Bezier','PolyDegree',5,...
-        'RelativeDegree',2,'OutputLabel',{y2_label},'PhaseType','TimeBased',...
+        'RelativeDegree',2,'OutputLabel',{y2_label},'PhaseType','StateBased',...
         'PhaseVariable',tau,'PhaseParams',p,'Holonomic',true,'LoadPath',load_path);
 
 

art3/+sys/+domains/RightStance.m

@@ -58,10 +58,21 @@
     %     p = SymVariable('p',[2,1]);
     %     tau = (deltaPhip-p(2))/(p(1)-p(2));
 
-    % @time based phase variable
-    t = SymVariable('t');
+    % phase variable: linearized hip position
+    r_hip_frame = domain.Joints(getJointIndices(domain,'r_hip_pitch'));
+    p_rhp = getCartesianPosition(domain, r_hip_frame);
+    p_rf = getCartesianPosition(domain, right_foot);
+    p_hip = p_rhp(1) - p_rf(1);
+    deltaPhip = linearize(p_hip,x);
+
+    % @state based phase variable
     p = SymVariable('p',[2,1]);
-    tau = (t-p(1))/(p(2)-p(1));
+    tau = (deltaPhip-p(1))/(p(2)-p(1));
+
+%     % @time based phase variable
+%     t = SymVariable('t');
+%     p = SymVariable('p',[2,1]);
+%     tau = (t-p(1))/(p(2)-p(1));
 
 
     % relative degree two outputs:
@@ -85,7 +96,7 @@
         };
 
     y2 = VirtualConstraint(domain,ya_2,'position','DesiredType','Bezier','PolyDegree',5,...
-        'RelativeDegree',2,'OutputLabel',{y2_label},'PhaseType','TimeBased',...
+        'RelativeDegree',2,'OutputLabel',{y2_label},'PhaseType','StateBased',...
         'PhaseVariable',tau,'PhaseParams',p,'Holonomic',true,'LoadPath',load_path);
 
 

art3/bezier_fit.m

@@ -0,0 +1,8 @@
+function y = bezier_fit(gait_x, a, b, c, d, e, f)
+
+    y = zeros(size(gait_x));
+    P = [a b c d e f];
+    for i = 0:5
+        y = y + (P(1, i+1)*factorial(5)/(factorial(i)*factorial(5-i))).*((gait_x.^i).*(ones(size(gait_x))-gait_x).^(5-i));
+    end
+end

art3/main_opt.m

@@ -27,7 +27,7 @@
 % time by 7-10 faster. 
 % Set it to false for the first time, and save expressions after loaded the
 % model. 
-load_sym  = false; % if true, it will load symbolic expression from 
+load_sym  = true; % if true, it will load symbolic expression from 
 if load_sym    
     load_path   = 'gen/sym'; % path to export binary Mathematica symbolic expression (MX) files
     utils.init_path(load_path);
@@ -101,7 +101,7 @@
 
 
 %% update initial condition if use pre-existing gaits
-param = load('local/tmp_gait.mat');
+param = load('local/library/gait_X0.8.mat');
 opt.updateInitCondition(nlp,param.gait);
 
 
@@ -113,16 +113,15 @@
 % 3. warm start using use existing solutoin as the initial guess
 %[gait, sol, info] = opt.solve(nlp, sol, info); % if use previous solution "sol"
 [gait, sol, info] = opt.solve(nlp);
-
 
 
 %% save
-save('local/tmp_gait.mat','gait','sol','info','bounds');
+save('local/state_base_gait.mat','gait','sol','info','bounds');
 
 
 
 %% animation
-anim = plot.LoadOptAnimator(robot, gait,'SkipExporting', false); % set 'SkipExporting' = false, only for the first time!
+anim = plot.LoadOptAnimator(robot, gait,'SkipExporting', true); % set 'SkipExporting' = false, only for the first time!