Merge pull request #5 from mitdrc/drake-heel-raise

Drake heel raise

examples/Atlas/Atlas.m

@@ -19,12 +19,14 @@
       end
 
       addpath(fullfile(getDrakePath,'examples','Atlas','frames'));
-
-      w = warning('off','Drake:RigidBodyManipulator:UnsupportedVelocityLimits');
+
+      if ~isfield(options,'control_rate')
+        options.control_rate = 250;
+      end
+
       obj = obj@TimeSteppingRigidBodyManipulator(urdf,options.dt,options);
-      obj = obj@Biped('r_foot_sole', 'l_foot_sole');
-      warning(w);
-
+      obj = obj@Biped('r_foot', 'l_foot','r_foot_sole', 'l_foot_sole');
+
       if options.floating
         % could also do fixed point search here
         obj = obj.setInitialState(obj.resolveConstraints(zeros(obj.getNumStates(),1)));
@@ -34,6 +36,14 @@
         obj.manip = compile(obj.manip);
         obj = obj.setInitialState(zeros(obj.getNumStates(),1));
       end
+
+      obj.left_full_support = RigidBodySupportState(obj,obj.foot_body_id.left);
+      obj.left_toe_support = RigidBodySupportState(obj,obj.foot_body_id.left,{{'toe'}});
+      obj.right_full_support = RigidBodySupportState(obj,obj.foot_body_id.right);
+      obj.right_toe_support = RigidBodySupportState(obj,obj.foot_body_id.right,{{'toe'}});
+      obj.left_full_right_full_support = RigidBodySupportState(obj,[obj.foot_body_id.left,obj.foot_body_id.right]);
+      obj.left_toe_right_full_support = RigidBodySupportState(obj,[obj.foot_body_id.left,obj.foot_body_id.right],{{'toe'},{'heel','toe'}});
+      obj.left_full_right_toe_support = RigidBodySupportState(obj,[obj.foot_body_id.left,obj.foot_body_id.right],{{'heel','toe'},{'toe'}});
     end
 
     function obj = compile(obj)
@@ -75,18 +85,18 @@
     function weights = getFootstepOptimizationWeights(obj)
       % Return a reasonable set of default weights for the footstep planner
       % optimization. The weights describe the following quantities:
-      % 'relative': the contribution to the cost function of the 
-      %             displacement from one step to the next 
+      % 'relative': the contribution to the cost function of the
+      %             displacement from one step to the next
       % 'relative_final': the cost contribution of the displacement of the
-      %                   displacement of the very last step (this can be 
+      %                   displacement of the very last step (this can be
       %                   larger than the normal 'relative' cost in
       %                   order to encourage the feet to be close together
       %                   at the end of a plan)
       % 'goal': the cost contribution on the distances from the last two
       %         footsteps to their respective goal poses.
       % Each weight is a 6 element vector, describing the weights on
       % [x, y, z, roll, pitch, yaw]
-      
+
       weights = struct('relative', [1;1;1;0;0;0.5],...
                        'relative_final', [10;10;10;0;0;1],...
                        'goal', [100;100;0;0;0;10]);
@@ -193,5 +203,13 @@
   end
   properties
     fixed_point_file = fullfile(getDrakePath(), 'examples', 'Atlas', 'data', 'atlas_fp.mat');
+    % preconstructing these for efficiency
+    left_full_support
+    left_toe_support
+    right_full_support
+    right_toe_support
+    left_full_right_full_support
+    left_toe_right_full_support
+    left_full_right_toe_support
   end
 end

examples/Atlas/controllers/BodyMotionControlBlock.m

@@ -75,19 +75,32 @@
     end
 
     function y=output(obj,t,~,x)
-      link_con = obj.controller_data.link_constraints;
-      ind = [link_con.link_ndx]==obj.body_ind;
-      body_des = fasteval(link_con(ind).traj,t);
-      if isfield(link_con(ind),'dtraj') && ~isempty(link_con(ind).dtraj)
-        body_v_des = fasteval(link_con(ind).dtraj,t);
+      ctrl_data = obj.controller_data;
+
+      if isfield(ctrl_data.link_constraints,'traj')
+        ind = [ctrl_data.link_constraints.link_ndx]==obj.body_ind;
+        body_des = fasteval(ctrl_data.link_constraints(ind).traj,t);
+        if isfield(ctrl_data.link_constraints(ind),'dtraj') && ~isempty(ctrl_data.link_constraints(ind).dtraj)
+          body_v_des = fasteval(ctrl_data.link_constraints(ind).dtraj,t);
+        else
+          body_v_des = [0;0;0;0;0;0];
+        end
+        if isfield(ctrl_data.link_constraints(ind),'ddtraj') && ~isempty(ctrl_data.link_constraints(ind).ddtraj)
+          body_vdot_des = fasteval(ctrl_data.link_constraints(ind).ddtraj,t);
+        else
+          body_vdot_des = [0;0;0;0;0;0];
+        end         
       else
-        body_v_des = [0;0;0;0;0;0];
+        link_con_ind = [ctrl_data.link_constraints.link_ndx]==obj.body_ind;
+        body_traj_ind = find(ctrl_data.link_constraints(link_con_ind).ts<=t,1,'last');
+        tt = t-ctrl_data.link_constraints(link_con_ind).ts(body_traj_ind);
+        a0 = ctrl_data.link_constraints(link_con_ind).a0(:,body_traj_ind);
+        a1 = ctrl_data.link_constraints(link_con_ind).a1(:,body_traj_ind);
+        a2 = ctrl_data.link_constraints(link_con_ind).a2(:,body_traj_ind);
+        a3 = ctrl_data.link_constraints(link_con_ind).a3(:,body_traj_ind);
+        [body_des,body_v_des,body_vdot_des] = evalCubicSplineSegment(tt,a0,a1,a2,a3);
       end
-      if isfield(link_con(ind),'ddtraj') && ~isempty(link_con(ind).ddtraj)
-        body_vdot_des = fasteval(link_con(ind).ddtraj,t);
-      else
-        body_vdot_des = [0;0;0;0;0;0];
-      end 
+
       if (obj.use_mex == 0 || obj.use_mex==2)
         q = x(1:obj.nq);
         qd = x(obj.nq+1:end);

examples/Atlas/controllers/FootContactBlock.m

@@ -5,12 +5,12 @@
     num_outputs;
     mex_ptr;
     controller_data;
-    lfoot_idx;
-    rfoot_idx;
     using_flat_terrain; % true if using DRCFlatTerrain
     contact_threshold; % min height above terrain to be considered in contact
     use_lcm;
     use_contact_logic_OR;
+    robot;
+    nq;
   end
 
   methods
@@ -121,14 +121,13 @@
       end      
       obj.mex_ptr = SharedDataHandle(supportDetectmex(0,r.getMexModelPtr.ptr,terrain_map_ptr));
 
-      obj.rfoot_idx = findLinkInd(r,'r_foot');
-      obj.lfoot_idx = findLinkInd(r,'l_foot');
-
       if isa(getTerrain(r),'DRCFlatTerrainMap')
         obj.using_flat_terrain = true;      
       else
         obj.using_flat_terrain = false;
       end
+      obj.robot = r;
+      obj.nq = getNumPositions(r);
 
     end
 
@@ -145,8 +144,29 @@
           breaking_contact = length(supp_prev.bodies)>length(supp.bodies);
           contact_logic_AND = breaking_contact;
         end  
+        if supp_idx < length(ctrl_data.support_times) 
+          supp_next = ctrl_data.supports(supp_idx+1);
+          t0 = ctrl_data.support_times(supp_idx);
+          t1 = ctrl_data.support_times(supp_idx+1);
+          if any(supp.bodies==obj.robot.foot_body_id.left) && any(supp.bodies==obj.robot.foot_body_id.right)
+            % double support
+            if any(supp_next.bodies==obj.robot.foot_body_id.right) && ~any(supp_next.bodies==obj.robot.foot_body_id.left)
+              obj.controller_data.pelvis_height_foot_reference = 1-(t1-t)/(t1-t0); % going into right contact
+            end
+            if any(supp_next.bodies==obj.robot.foot_body_id.left) && ~any(supp_next.bodies==obj.robot.foot_body_id.right)
+              obj.controller_data.pelvis_height_foot_reference = (t1-t)/(t1-t0); % going into left contact
+            end
+          elseif any(supp.bodies==obj.robot.foot_body_id.left)
+            % left support
+            obj.controller_data.pelvis_height_foot_reference = 0; % left
+          elseif any(supp.bodies==obj.robot.foot_body_id.right)
+            % right support
+            obj.controller_data.pelvis_height_foot_reference = 1; % right
+          end
+        end
       else      
         supp = ctrl_data.supports;
+        obj.controller_data.pelvis_height_foot_reference = 0; % left
       end
 
       % contact_sensor = -1 (no info), 0 (info, no contact), 1 (info, yes contact)
@@ -157,8 +177,8 @@
         contact_data = obj.contact_est_monitor.getMessage(); % slow
         if ~isempty(contact_data)
           msg = drc.foot_contact_estimate_t(contact_data);
-          contact_sensor(supp.bodies==obj.lfoot_idx) = msg.left_contact > 0.5;
-          contact_sensor(supp.bodies==obj.rfoot_idx) = msg.right_contact > 0.5;
+          contact_sensor(supp.bodies==obj.robot.foot_body_id.left) = msg.left_contact > 0.5;
+          contact_sensor(supp.bodies==obj.robot.foot_body_id.right) = msg.right_contact > 0.5;
         end
       end      
 
@@ -175,7 +195,7 @@
 
       active_supports = supportDetectmex(obj.mex_ptr.data,x,supp,contact_sensor,contact_thresh,height,contact_logic_AND);
 
-      y = [1.0*any(active_supports==obj.lfoot_idx); 1.0*any(active_supports==obj.rfoot_idx)];
+      y = [1.0*any(active_supports==obj.robot.foot_body_id.left); 1.0*any(active_supports==obj.robot.foot_body_id.right)];
       if obj.num_outputs > 1
         varargout = cell(1,obj.num_outputs);
         for i=1:obj.num_outputs

examples/Atlas/controllers/PelvisMotionControlBlock.m

@@ -10,8 +10,6 @@
     controller_data; 
     robot;
     body_ind;
-    rfoot_ind;
-    lfoot_ind;
     mex_ptr;
     alpha;
     nominal_pelvis_height;
@@ -74,25 +72,24 @@
       end
 
       obj.alpha = 0.95;
-      obj.nominal_pelvis_height = 0.75;
+      obj.nominal_pelvis_height = 0.76;
       obj.robot = r;
 
       if (obj.use_mex>0)
         obj.mex_ptr = SharedDataHandle(pelvisMotionControlmex(0,obj.robot.getMexModelPtr.ptr,obj.alpha,obj.nominal_pelvis_height,obj.Kp,obj.Kd));
       end
 
       obj.body_ind = findLinkInd(r,name);
-      obj.lfoot_ind = findLinkInd(r,'l_foot');
-      obj.rfoot_ind = findLinkInd(r,'r_foot');
     end
 
     function y=output(obj,t,~,x)
       ctrl_data = obj.controller_data;
-      lfoot_link_con_ind = [ctrl_data.link_constraints.link_ndx]==obj.lfoot_ind;
-      rfoot_link_con_ind = [ctrl_data.link_constraints.link_ndx]==obj.rfoot_ind;
-      lfoot_des = fasteval(ctrl_data.link_constraints(lfoot_link_con_ind).traj,t);
-      rfoot_des = fasteval(ctrl_data.link_constraints(rfoot_link_con_ind).traj,t);
+      lfoot_link_con_ind = [ctrl_data.link_constraints.link_ndx]==obj.robot.foot_body_id.left;
+      rfoot_link_con_ind = [ctrl_data.link_constraints.link_ndx]==obj.robot.foot_body_id.right;
 
+      lfoot_des = evaluateSplineInLinkConstraints(t,ctrl_data.link_constraints,lfoot_link_con_ind);
+      rfoot_des = evaluateSplineInLinkConstraints(t,ctrl_data.link_constraints,rfoot_link_con_ind);
+
       if (obj.use_mex == 0 || obj.use_mex == 2)
         q = x(1:obj.nq);
         qd = x(obj.nq+1:end);
@@ -101,13 +98,12 @@
         % TODO: this must be updated to use quaternions/spatial velocity
         [p,J] = forwardKin(obj.robot,kinsol,obj.body_ind,[0;0;0],1); 
 
-        lfoot = forwardKin(obj.robot,kinsol,obj.lfoot_ind,[0;0;0],0);
-        rfoot = forwardKin(obj.robot,kinsol,obj.rfoot_ind,[0;0;0],0);
-
+        foot_z = ctrl_data.pelvis_foot_height_reference;
+
         if isempty(obj.controller_data.pelvis_z_prev)
           obj.controller_data.pelvis_z_prev = p(3);
         end
-        z_des = obj.alpha*obj.controller_data.pelvis_z_prev + (1-obj.alpha)*(min([lfoot(3),rfoot(3)])+obj.nominal_pelvis_height); % X cm above feet
+        z_des = obj.alpha*obj.controller_data.pelvis_z_prev + (1-obj.alpha)*(foot_z+obj.nominal_pelvis_height); % X cm above feet
         obj.controller_data.pelvis_z_prev = z_des;
 
         body_des = [nan;nan;z_des;0;0;angleAverage(lfoot_des(6),rfoot_des(6))]; 
@@ -116,11 +112,11 @@
         body_vdot = obj.Kp.*err - obj.Kd.*(J*qd);
         if obj.use_mex == 2
           % check that matlab/mex agree
-          body_vdot_mex = pelvisMotionControlmex(obj.mex_ptr.data,x,lfoot_des(6),rfoot_des(6));  
+          body_vdot_mex = pelvisMotionControlmex(obj.mex_ptr.data,x,lfoot_des(6),rfoot_des(6),ctrl_data.pelvis_foot_height_reference);  
           valuecheck(body_vdot_mex,body_vdot);
         end
       else
-        body_vdot = pelvisMotionControlmex(obj.mex_ptr.data,x,lfoot_des(6),rfoot_des(6));  
+        body_vdot = pelvisMotionControlmex(obj.mex_ptr.data,x,lfoot_des(6),rfoot_des(6),ctrl_data.pelvis_foot_height_reference);  
       end
       y = [obj.body_ind;body_vdot];
     end

examples/Pendulum/PendulumInput.m

@@ -2,7 +2,7 @@
 
   methods
     function obj=PendulumInput()
-      obj = obj@LCMCoordinateFrame('PendulumInput','drake.examples.Pendulum.lcmt_pendulum_u','u');
+      obj = obj@LCMCoordinateFrame('PendulumInput','drake.examples.Pendulum.lcmt_pendulum_u','u',{'torque'});
       obj = obj@Singleton();
     end
   end

examples/Pendulum/PendulumPlant.m

@@ -14,10 +14,14 @@
   end
 
   methods
-    function obj = PendulumPlant()
+    function obj = PendulumPlant(b)
       % Construct a new PendulumPlant
       obj = obj@SecondOrderSystem(1,1,true);
 
+      if nargin>0 && ~isempty(b) % accept damping as optional input
+        obj.b = b;
+      end
+
       obj = setInputFrame(obj,PendulumInput);
       torque_limit = 3;
       obj = setInputLimits(obj,-torque_limit,torque_limit);

examples/Pendulum/PendulumState.m

@@ -2,7 +2,7 @@
 
   methods
     function obj=PendulumState()
-      obj = obj@LCMCoordinateFrame('PendulumState','drake.examples.Pendulum.lcmt_pendulum_x','x');
+      obj = obj@LCMCoordinateFrame('PendulumState','drake.examples.Pendulum.lcmt_pendulum_x','x',{'theta','thetadot'});
       obj = obj@Singleton();
     end
   end