Merge pull request RobotLocomotion#45 from mitdrc/rd-manip-fixes-drake

Rd manip fixes drake
juanfxlopez · Mar 20, 2015 · 1c92de0 · 1c92de0
2 parents 47e1e1a + d858eca
commit 1c92de0
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 6 deletions.
diff --git a/examples/Atlas/+atlasControllers/QPInputConstantHeight.m b/examples/Atlas/+atlasControllers/QPInputConstantHeight.m
@@ -29,7 +29,7 @@
                         'y0', zeros(2,1),... % nominal output % 2 d
                         'u0', zeros(2,1),... % nominal input % 2 d
                         'R', zeros(2),... % input LQR cost % 4 d
-                        'Qy', eye(2),... % output LQR cost % 4 d
+                        'Qy', 0.8*eye(2),... % output LQR cost % 4 d
                         'S', zeros(4),... % cost-to-go terms: x'Sx + x's1 + s2 % 16 d
                         's1', zeros(4,1),... % 4 d
                         's1dot', zeros(4,1),... % 4 d

diff --git a/systems/robotInterfaces/QPLocomotionPlan.m b/systems/robotInterfaces/QPLocomotionPlan.m
@@ -55,6 +55,10 @@
       r = obj.robot;
       t_plan = t_global - obj.start_time;
       t_plan = double(t_plan);
+      if t_plan < 0
+        qp_input = [];
+        return;
+      end
 
       T = obj.duration;
       t_plan = min([t_plan, T]);
@@ -84,7 +88,13 @@
         qp_input.whole_body_data.q_des = fasteval(obj.qtraj, t_plan);
       end
 
-      supp_idx = find(obj.support_times<=t_plan,1,'last');
+      if t_plan < obj.support_times(1)
+        supp_idx = 1;
+      elseif t_plan > obj.support_times(end)
+        supp_idx = length(obj.support_times);
+      else
+        supp_idx = find(obj.support_times<=t_plan,1,'last');
+      end
 
       MIN_KNEE_ANGLE = 0.7;
       KNEE_KP = 40;
@@ -98,10 +108,12 @@
         if qp_input.body_motion_data(j).body_id == rpc.body_ids.pelvis
           pelvis_has_tracking = true;
         end
-        if t_plan <= obj.link_constraints(j).ts(end)
-          body_t_ind = find(obj.link_constraints(j).ts<=t_plan,1,'last');
-        else
+        if t_plan < obj.link_constraints(j).ts(1)
+          body_t_ind = 1;
+        elseif t_plan > obj.link_constraints(j).ts(end)
           body_t_ind = length(obj.link_constraints(j).ts);
+        else
+          body_t_ind = find(obj.link_constraints(j).ts<=t_plan,1,'last');
         end
         if body_t_ind < length(obj.link_constraints(j).ts)
           qp_input.body_motion_data(j).ts = obj.link_constraints(j).ts(body_t_ind:body_t_ind+1) + obj.start_time;
@@ -330,7 +342,7 @@ function plot_traj_foh(traj, color)
       link_constraints(3).pt = [0;0;0];
       link_constraints(3).ts = [0, inf];
       pelvis_current = forwardKin(obj.robot,kinsol,pelvis_id,[0;0;0],1);
-      pelvis_target = pelvis_current;
+      pelvis_target = [mean(foot_pos(1:2,:), 2); pelvis_current(3:end)];
       link_constraints(3).coefs = cat(3, zeros(6,1,3),reshape(pelvis_target,[6,1,1,]));
       obj.link_constraints = link_constraints;