Merge pull request RobotLocomotion#110 from rdeits/rd-undershoot-flag

new flag to explicitly prevent over/undershoot in swing

examples/Atlas/Atlas.m

@@ -393,6 +393,8 @@
                                     'constrain_full_foot_pose', true,... % whether to constrain the swing foot roll and pitch
                                     'pelvis_height_above_foot_sole', 0.83,... % default pelvis height when walking
                                     'support_contact_groups', {{'heel', 'toe'}},... % which contact groups are available for support when walking
+                                    'prevent_swing_undershoot', false,... % prevent the first phase of the swing from going backwards while moving to the first knot point
+                                    'prevent_swing_overshoot', false,... % prevent the final phase of the swing from moving forward of the last knot point
                                     'nominal_LIP_COM_height', 0.89); % nominal height used to construct D_ls for our linear inverted pendulum model
   end
 

systems/robotInterfaces/@Biped/planSwingPitched.m

@@ -143,7 +143,7 @@ function add_frame_knot(swing_pose, speed)
 % Apex knot 1
 toe_apex1_in_world = (1-APEX_FRACTIONS(1))*toe1(1:3) + APEX_FRACTIONS(1)*toe2(1:3);
 initial_pose_not_flat = norm(cross(T_swing1_sole_to_world(1:3,1:3) * [0;0;1], [0;0;1])) >= sin(7.5*pi/180);
-if initial_pose_not_flat || max_terrain_ht_in_world > toe_apex1_in_world(3) + params.step_height/4
+if (initial_pose_not_flat || max_terrain_ht_in_world > toe_apex1_in_world(3) + params.step_height/4) && (~params.prevent_swing_undershoot)
   toe_apex1_in_world = [toe1(1:2); max_terrain_ht_in_world + params.step_height];
 else
   toe_apex1_in_world(3) = max([toe_apex1_in_world(3) + params.step_height,...
@@ -156,7 +156,7 @@ function add_frame_knot(swing_pose, speed)
 % Apex knot 2
 toe_apex2_in_world = (1-APEX_FRACTIONS(2))*toe1(1:3) + APEX_FRACTIONS(2)*toe2(1:3);
 final_pose_not_flat = norm(cross(T_swing2_sole_to_world(1:3,1:3) * [0;0;1], [0;0;1])) >= sin(7.5*pi/180);
-if final_pose_not_flat || max_terrain_ht_in_world > toe_apex2_in_world(3) + params.step_height/4
+if (final_pose_not_flat || max_terrain_ht_in_world > toe_apex2_in_world(3) + params.step_height/4) && (~params.prevent_swing_overshoot)
   toe_apex2_in_world = [toe2(1:2); max_terrain_ht_in_world + params.step_height];
 else
   toe_apex2_in_world(3) = max([toe_apex2_in_world(3) + params.step_height,...