Merge branch 'master' of https://github.com/RobotLocomotion/drake int…

…o urdf_fix

addpath_drake.m

@@ -52,6 +52,7 @@
 addpath(fullfile(conf.root,'systems','trajectories'));
 addpath(fullfile(conf.root,'systems','frames'));
 addpath(fullfile(conf.root,'systems','visualizers'));
+addpath(fullfile(conf.root,'systems','robotInterfaces'));
 addpath(fullfile(conf.root,'solvers'));
 addpath(fullfile(conf.root,'util'));
 addpath(fullfile(conf.root,'util','obstacles'));

examples/Atlas/Atlas.m

@@ -1,7 +1,7 @@
-classdef Atlas < TimeSteppingRigidBodyManipulator
-  
+classdef Atlas < TimeSteppingRigidBodyManipulator & Biped
+
   methods
-    
+
     function obj=Atlas(urdf,options)
       typecheck(urdf,'char');
 
@@ -17,13 +17,14 @@
       if ~isfield(options,'terrain')
         options.terrain = RigidBodyFlatTerrain;
       end
-      
+
       addpath(fullfile(getDrakePath,'examples','Atlas','frames'));
-  
+
       w = warning('off','Drake:RigidBodyManipulator:UnsupportedVelocityLimits');
       obj = obj@TimeSteppingRigidBodyManipulator(urdf,options.dt,options);
+      obj = obj@Biped('r_foot_sole', 'l_foot_sole');
       warning(w);
-      
+
       if options.floating
         % could also do fixed point search here
         obj = obj.setInitialState(obj.resolveConstraints(zeros(obj.getNumStates(),1)));
@@ -34,7 +35,7 @@
         obj = obj.setInitialState(zeros(obj.getNumStates(),1));
       end
     end
-    
+
     function obj = compile(obj)
       obj = compile@TimeSteppingRigidBodyManipulator(obj);
 
@@ -50,7 +51,7 @@
       obj.manip = obj.manip.setOutputFrame(atlas_state_frame);
       obj = obj.setStateFrame(state_frame);
       obj = obj.setOutputFrame(state_frame);
-    
+
       input_frame = AtlasInput(obj);
       obj = obj.setInputFrame(input_frame);
       obj.manip = obj.manip.setInputFrame(input_frame);
@@ -65,16 +66,16 @@
         obj.x0 = x0;
       end
     end
-    
+
     function x0 = getInitialState(obj)
       x0 = obj.x0;
-    end    
-    
+    end
+
     function [xstar,ustar,zstar] = getFixedPoint(obj,options)
       if nargin < 2 || ~isfield(options,'visualize')
         options.visualize = false;
       end
-      
+
       x0 = Point(obj.getStateFrame());
       x0 = resolveConstraints(obj,x0);
       u0 = zeros(obj.getNumInputs(),1);
@@ -84,7 +85,7 @@
       nz = obj.getNumContacts()*3;
       z0 = zeros(nz,1);
       q0 = x0(1:nq);
-    
+
       problem.x0 = [q0;u0;z0];
       problem.objective = @(quz) 0; % feasibility problem
       problem.nonlcon = @(quz) mycon(quz);
@@ -97,11 +98,11 @@
       else
         problem.options=optimset('GradConstr','on','Algorithm','interior-point','TolX',1e-14,'MaxFunEvals',5000);
       end
-      
+
       lb_z = -1e6*ones(nz,1);
       lb_z(3:3:end) = 0; % normal forces must be >=0
       ub_z = 1e6*ones(nz,1);
-    
+
       [jl_min,jl_max] = obj.getJointLimits();
       % force search to be close to starting position
       problem.lb = [max(q0-0.05,jl_min+0.01); obj.umin; lb_z];
@@ -130,24 +131,44 @@
         [~,C,B,~,dC,~] = obj.manip.manipulatorDynamics(q,zeros(nq,1));
         [phiC,JC] = obj.contactConstraints(q);
         [~,J,dJ] = obj.contactPositions(q);
-        
+
         % ignore friction constraints for now
         c = 0;
-        GC = zeros(nq+nu+nz,1); 
-        
+        GC = zeros(nq+nu+nz,1);
+
         dJz = zeros(nq,nq);
         for i=1:nq
             dJz(:,i) = dJ(:,(i-1)*nq+1:i*nq)'*z;
         end
-        
+
         ceq = [C-B*u-J'*z; phiC];
-        GCeq = [[dC(1:nq,1:nq)-dJz,-B,-J']',[JC'; zeros(nu+nz,length(phiC))]]; 
+        GCeq = [[dC(1:nq,1:nq)-dJz,-B,-J']',[JC'; zeros(nu+nz,length(phiC))]];
       end
     end
-      
+
   end
-  
+
   properties (SetAccess = protected, GetAccess = public)
     x0
+    default_footstep_params = struct('nom_forward_step', 0.15,... % m
+                                      'max_forward_step', 0.3,...% m
+                                      'max_step_width', 0.40,...% m
+                                      'min_step_width', 0.21,...% m
+                                      'nom_step_width', 0.26,...% m
+                                      'max_outward_angle', pi/8,... % rad
+                                      'max_inward_angle', 0.01,... % rad
+                                      'nom_upward_step', 0.2,... % m
+                                      'nom_downward_step', 0.2,...% m
+                                      'max_num_steps', 10,...
+                                      'min_num_steps', 1);
+    default_walking_params = struct('step_speed', 0.5,... % speed of the swing foot (m/s)
+                                    'step_height', 0.05,... % approximate clearance over terrain (m)
+                                    'hold_frac', 0.4,... % fraction of the swing time spent in double support
+                                    'drake_min_hold_time', 1.0,... % minimum time in double support (s)
+                                    'mu', 1.0,... % friction coefficient
+                                    'constrain_full_foot_pose', true); % whether to constrain the swing foot roll and pitch
+  end
+  properties
+    fixed_point_file = fullfile(getDrakePath(), 'examples', 'Atlas', 'data', 'atlas_fp.mat');
   end
 end

examples/Atlas/runAtlasFootstepPlanning.m

@@ -0,0 +1,36 @@
+function plan = runAtlasFootstepPlanning()
+% Demonstration of footstep planning for the Atlas biped. We choose to define the footstep
+% planning problem as one of finding a set of sequential foot positions which are
+% kinematically reachable and which are *likely* to yield a dynamically stable walking
+% motion. See also: runAtlasWalkingPlanning for the full dynamical plan.
+% @retval plan a FootstepPlan describing the sequence of foot positions
+
+
+% Set up the model
+load('data/atlas_fp.mat', 'xstar');
+r = Atlas('urdf/atlas_minimal_contact.urdf');
+r = r.setInitialState(xstar);
+
+% Find the initial positions of the feet
+kinsol = doKinematics(r, xstar(1:r.getNumDOF()));
+start_pos = struct('right', forwardKin(r, kinsol, r.foot_frame_id.right, [0;0;0], 1), ...
+                   'left', forwardKin(r, kinsol, r.foot_frame_id.left, [0;0;0], 1));
+
+% Plan footsteps to the goal
+goal_pos = struct('right', [1;0;0;0;0;0], 'left', [1;0.26;0;0;0;0]);
+plan = r.planFootsteps(start_pos, goal_pos);
+
+% Show the result
+v = r.constructVisualizer();
+v.draw(0, xstar);
+if isa(v, 'BotVisualizer')
+  checkDependency('lcmgl');
+  lcmgl = drake.util.BotLCMGLClient(lcm.lcm.LCM.getSingleton(), 'footstep_plan');
+  plan.draw_lcmgl(lcmgl);
+else
+  figure(25)
+  plan.draw_2d();
+end
+
+end
+

examples/Atlas/runAtlasWalkingPlanning.m

@@ -0,0 +1,45 @@
+function plan = runAtlasWalkingPlanning()
+% Demonstration of footstep and walking planning on Atlas. First, the footstep planning
+% step generates a set of reachable footstep poses. Next, we use those foot poses to
+% plan a trajectory of the Zero Moment Point (ZMP), from which we can derive a
+% trajectory for the robot's Center of Mass. From this, we can plan a full state
+% trajectory for the robot.
+% @retval plan a WalkingPlanData object including the zmp and com trajectories
+
+
+addpath(fullfile(getDrakePath(), 'examples', 'ZMP'));
+% Set up the model
+load('data/atlas_fp.mat', 'xstar');
+x0 = xstar;
+r = Atlas('urdf/atlas_minimal_contact.urdf');
+r = r.setInitialState(x0);
+
+
+% Plan footsteps to the goal
+q0 = x0(1:r.getNumDOF());
+goal_pos = struct('right', [1;0;0;0;0;0], 'left', [1;0.26;0;0;0;0]);
+footstep_plan = r.planFootsteps(q0, goal_pos);
+
+% Show the result
+v = r.constructVisualizer();
+v.draw(0, x0);
+walking_plan_data = r.planWalkingZMP(x0, footstep_plan);
+[xtraj, htraj, ts] = r.planWalkingStateTraj(walking_plan_data);
+
+if isa(v, 'BotVisualizer')
+  checkDependency('lcmgl');
+  lcmgl = drake.util.BotLCMGLClient(lcm.lcm.LCM.getSingleton(), 'footstep_plan');
+  footstep_plan.draw_lcmgl(lcmgl);
+  lcmgl = drake.util.BotLCMGLClient(lcm.lcm.LCM.getSingleton(), 'walking_plan');
+  walking_plan_data.draw_lcmgl(lcmgl);
+else
+  figure(25)
+  footstep_plan.draw_2d();
+end
+
+v.playback(xtraj);
+
+
+
+end
+

rmpath_drake.m

@@ -18,6 +18,7 @@
 rmpath(fullfile(conf.root,'systems','trajectories'));
 rmpath(fullfile(conf.root,'systems','frames'));
 rmpath(fullfile(conf.root,'systems','visualizers'));
+rmpath(fullfile(conf.root,'systems','robotInterfaces'));
 rmpath(fullfile(conf.root,'solvers'));
 rmpath(fullfile(conf.root,'util'));
 rmpath(fullfile(conf.root,'util','obstacles'));

systems/CMakeLists.txt

@@ -4,4 +4,5 @@ add_mex(DCSFunction DCSFunction.cpp)
 add_subdirectory(plants)
 add_subdirectory(trajectories)
 add_subdirectory(controllers)
+add_subdirectory(robotInterfaces)
 

systems/plants/RigidBodySupportState.m

@@ -1,22 +1,22 @@
 classdef RigidBodySupportState
   %RigidBodySupportState defines a set of supporting bodies and contacts for a rigid
   %  body manipulator
-  
+
   properties (SetAccess=protected)
     bodies; % array of supporting body indices
     contact_pts; % cell array of supporting contact point indices
-    num_contact_pts;  % convenience array containing the desired number of 
+    num_contact_pts;  % convenience array containing the desired number of
                       %             contact points for each support body
     contact_surfaces; % int IDs either: 0 (terrain), -1 (any body in bullet collision world)
                       %             or (1:num_bodies) collision object ID
   end
-  
+
   methods
     function obj = RigidBodySupportState(r,bodies,contact_pts,contact_surfaces)
-      typecheck(r,'Atlas');
+      typecheck(r,'Biped');
       typecheck(bodies,'double');
       obj.bodies = bodies(bodies~=0);
-      
+
       obj.num_contact_pts=zeros(length(obj.bodies),1);
       if nargin>2
         typecheck(contact_pts,'cell');
@@ -39,42 +39,42 @@
           obj.num_contact_pts(i)=length(obj.contact_pts{i});
         end
       end
-      
+
       if nargin>3
         obj = setContactSurfaces(obj,contact_surfaces);
       else
         obj.contact_surfaces = zeros(length(obj.bodies),1);
       end
     end
-    
+
     function obj = setContactSurfaces(obj,contact_surfaces)
       typecheck(contact_surfaces,'double');
       sizecheck(contact_surfaces,length(obj.bodies));
       obj.contact_surfaces = contact_surfaces;
     end
-    
+
     function pts = contactPositions(obj,r,kinsol)
       pts=[];
       for i=1:length(obj.bodies)
         bp = getTerrainContactPoints(r,obj.bodies(i));
         pts = [pts,forwardKin(r,kinsol,obj.bodies(i),bp.pts(:,obj.contact_pts{i}))];
       end
     end
-    
+
     function obj = removeBody(obj,index_into_bodies_not_body_idx)
       obj.bodies(index_into_bodies_not_body_idx)=[];
       obj.contact_pts(index_into_bodies_not_body_idx)=[]; % note: this is correct, even though it's a cell array
       obj.num_contact_pts(index_into_bodies_not_body_idx)=[];
       obj.contact_surfaces(index_into_bodies_not_body_idx)=[];
     end
-    
+
     function obj = removeBodyIdx(obj,body_idx)
       ind = find(obj.bodies==body_idx);
       if ~isempty(idx)
         obj = removeBody(obj,ind);
       end
     end
   end
-  
+
 end