merge after replacing shape with geometry

examples/Atlas/test/testRunningPlanner.m

@@ -20,8 +20,8 @@
   atlas_urdf = [getDrakePath,'/examples/Atlas/urdf/atlas_convex_hull.urdf'];
   atlas_vis_urdf = [getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'];
   robot = RigidBodyManipulator(atlas_urdf,options);
-  robot = robot.replaceContactShapesWithCHull(robot.findLinkInd('l_hand'),1);
-  robot = robot.replaceContactShapesWithCHull(robot.findLinkInd('r_hand'),1);
+  robot = robot.replaceCollisionGeometryWithConvexHull(robot.findLinkInd('l_hand'),1);
+  robot = robot.replaceCollisionGeometryWithConvexHull(robot.findLinkInd('r_hand'),1);
   robot = compile(robot);
   robot_vis = RigidBodyManipulator(atlas_vis_urdf,options);
   if options.add_obstacle
@@ -598,10 +598,10 @@ function displayCallback(in_stance,N,x)
   wall1 = RigidBodyBox(2*[0.1; 1.235; 1.0],[obstacle_x_position-0.2,-0.565-1,1],[0;0;0]);
   wall2 = RigidBodyBox(2*[0.1; 0.15; 1.0],[obstacle_x_position-0.2,1.65-1,1],[0;0;0]);
   wall3 = RigidBodyBox(2*[0.1; 1.8; 0.25],[obstacle_x_position-0.2,0-1,2.25],[0;0;0]);
-  robot = robot.addShapeToBody('world',beam);
-  robot = robot.addShapeToBody('world',wall1);
-  robot = robot.addShapeToBody('world',wall2);
-  robot = robot.addShapeToBody('world',wall3);
+  robot = robot.addGeometryToBody('world',beam);
+  robot = robot.addGeometryToBody('world',wall1);
+  robot = robot.addGeometryToBody('world',wall2);
+  robot = robot.addGeometryToBody('world',wall3);
   robot = compile(robot);
 end
 

examples/PlanarNLink/PlanarNLink.m

@@ -41,17 +41,17 @@
       body.linkname=['link',num2str(ind-1)];
       body.robotnum=1;
       body = setInertial(body,mass,[0;0;-len/2],diag([1;mass*len^2/12;1]));  % solid rod w/ uniform mass
-      body.visual_shapes{1} = RigidBodyCylinder(radius,len);
-      body.visual_shapes{1}.T = [eye(3),[0 0 -len/2]';zeros(1,3),1];
+      body.visual_geometry{1} = RigidBodyCylinder(radius,len);
+      body.visual_geometry{1}.T = [eye(3),[0 0 -len/2]';zeros(1,3),1];
       h=figure(1035); set(h,'Visible','off');
       co = get(gca,'ColorOrder');
       close(h);
-      body.visual_shapes{1}.c = co(mod(ind-2,size(co,1))+1,:);
+      body.visual_geometry{1}.c = co(mod(ind-2,size(co,1))+1,:);
       model.body = [model.body, body];
 
       % joint properties
       if (ind>2)
-        parentlen=model.body(ind-1).visual_shapes{1}.len;
+        parentlen=model.body(ind-1).visual_geometry{1}.len;
       else
         parentlen = 0;
       end

examples/Quadrotor/Quadrotor.m

@@ -43,10 +43,9 @@
         xy = randn(2,1);
         while(norm(xy)<1), xy = randn(2,1); end
         height = .5+rand;
-        shape = RigidBodyBox([.2+.8*rand(1,2) height],[xy;height/2],[0;0;randn]);
-        shape.c = rand(3,1);
-        obj = addShapeToBody(obj,'world',shape);
-        obj = addContactShapeToBody(obj,'world',shape);
+        geometry = RigidBodyBox([.2+.8*rand(1,2) height],[xy;height/2],[0;0;randn]);
+        geometry.c = rand(3,1);
+        obj = addGeometryToBody(obj,'world',geometry);
       end
 
       obj = compile(obj);
@@ -76,13 +75,11 @@
       treeTrunk = RigidBodyBox([.2+.8*width_param height],...
           [xy;height/2],[0;0;yaw]);
       treeTrunk.c = [83,53,10]/255;  % brown
-      obj = addShapeToBody(obj,'world',treeTrunk);
-      obj = addContactShapeToBody(obj,'world',treeTrunk);
+      obj = addGeometryToBody(obj,'world',treeTrunk);
       treeLeaves = RigidBodyBox(1.5*[.2+.8*width_param height/4],...
           [xy;height + height/8],[0;0;yaw]);
       treeLeaves.c = [0,0.7,0];  % green
-      obj = addShapeToBody(obj,'world',treeLeaves);
-      obj = addContactShapeToBody(obj,'world',treeLeaves);
+      obj = addGeometryToBody(obj,'world',treeLeaves);
       obj = compile(obj);
     end    
 
@@ -137,4 +134,4 @@ function visualizePlan(x,lcmgl)
 %      figure(1); clf; fnplt(ytraj);
     end
   end
-end
+end

examples/Quadrotor/runDircolWObs.m

@@ -27,7 +27,7 @@
 u0 = double(nominalThrust(r));
 
 %if (nargout<1)
-  v = constructVisualizer(r);%,struct('use_contact_shapes',true));
+  v = constructVisualizer(r);%,struct('use_collision_geometry',true));
   v.draw(0,double(x0));
 
   prog = addPlanVisualizer(r,prog);

examples/Quadrotor/runMixedIntegerEnvironment.m

@@ -31,9 +31,9 @@ function runMixedIntegerEnvironment(r, start, goal, lb, ub, seeds, traj_degree,
 v.draw(0,double(x0));
 
 b = r.getBody(1);
-obstacles = cell(1, length(b.getContactShapes()));
-for j = 1:length(b.getContactShapes())
-  obstacles{j} = b.getContactShapes{j}.getPoints();
+obstacles = cell(1, length(b.getCollisionGeometry()));
+for j = 1:length(b.getCollisionGeometry())
+  obstacles{j} = b.getCollisionGeometry{j}.getPoints();
 end
 
 if can_draw_lcm_polytopes

solvers/NonlinearProgram.m

@@ -917,6 +917,10 @@
       %                    -201  -- In ipopt, non-IPOPT exception thrown
       %                    -202  -- In ipopt, insufficient memory
       %                    -299  -- In ipopt, internal error
+      % 
+      % When using fmincon, if the algorithm is not specified through
+      % setSolverOptions('fmincon','Algorithm',ALGORITHM), then it will
+      % iterate all possible algorithms in fmincon to search for a solution.
       switch lower(obj.solver)
         case 'snopt'
           [x,objval,exitflag,infeasible_constraint_name] = snopt(obj,x0);
@@ -1346,8 +1350,21 @@ function checkGradient(x_free)
         dceq = dh';
       end
 
-      [x,objval,exitflag] = fmincon(@obj.objective,x0,full(obj.Ain),...
-        obj.bin,full(obj.Aeq),obj.beq,obj.x_lb,obj.x_ub,@fmincon_userfun,obj.solver_options.fmincon);
+      if(isempty(obj.solver_options.fmincon.Algorithm))
+        algorithms = {'interior-point','sqp','active-set','trust-region-reflective'};
+        fmincon_options = obj.solver_options.fmincon;
+        for i = 1:length(algorithms)
+          fmincon_options.Algorithm = algorithms{i};
+          [x,objval,exitflag] = fmincon(@obj.objective,x0,full(obj.Ain),...
+            obj.bin,full(obj.Aeq),obj.beq,obj.x_lb,obj.x_ub,@fmincon_userfun,fmincon_options);
+          if(exitflag == 1)
+            break;
+          end
+        end
+      else
+        [x,objval,exitflag] = fmincon(@obj.objective,x0,full(obj.Ain),...
+            obj.bin,full(obj.Aeq),obj.beq,obj.x_lb,obj.x_ub,@fmincon_userfun,obj.solver_options.fmincon);
+      end
       objval = full(objval);
 
 

solvers/trajectoryOptimization/ContactWrenchVisualizer.m

@@ -15,11 +15,11 @@
   end
 
   methods
-    function obj = ContactWrenchVisualizer(manip,t_knot,wrench_sol,use_contact_shapes)
+    function obj = ContactWrenchVisualizer(manip,t_knot,wrench_sol,use_collision_geometry)
       if nargin <4 
-        use_contact_shapes = false; 
+        use_collision_geometry = false; 
       end
-      obj = obj@BotVisualizer(manip,use_contact_shapes);
+      obj = obj@BotVisualizer(manip,use_collision_geometry);
       if(~isnumeric(t_knot))
         error('Drake:ContactWrenchVisualizer:t_knot should be numeric');
       end

solvers/trajectoryOptimization/dev/SalmonBarVisualizer.m

@@ -6,11 +6,11 @@
     r_hand_pt
   end
   methods
-    function obj = SalmonBarVisualizer(manip,t_knot,wrench_sol,l_hand,r_hand,l_hand_pt,r_hand_pt,use_contact_shapes)
+    function obj = SalmonBarVisualizer(manip,t_knot,wrench_sol,l_hand,r_hand,l_hand_pt,r_hand_pt,use_collision_geometry)
       if(nargin<8)
-        use_contact_shapes = false;
+        use_collision_geometry = false;
       end
-      obj = obj@ContactWrenchVisualizer(manip,t_knot,wrench_sol,use_contact_shapes);
+      obj = obj@ContactWrenchVisualizer(manip,t_knot,wrench_sol,use_collision_geometry);
       obj.l_hand = l_hand;
       obj.r_hand = r_hand;
       obj.l_hand_pt = l_hand_pt;
@@ -43,4 +43,4 @@ function drawBar(obj,lhand_pos,rhand_pos,bar_radius)
       lcmgl.switchBuffers();
     end
   end
-end
+end

solvers/trajectoryOptimization/dev/aggregateMonkeybarData.m

@@ -8,25 +8,25 @@
 l_foot = robot.findLinkInd('l_foot');
 r_foot = robot.findLinkInd('r_foot');
 utorso = robot.findLinkInd('utorso');
-l_foot_shapes_heel = robot.getBody(l_foot).getContactShapes('heel');
-l_foot_shapes_toe = robot.getBody(l_foot).getContactShapes('toe');
-r_foot_shapes_heel = robot.getBody(r_foot).getContactShapes('heel');
-r_foot_shapes_toe = robot.getBody(r_foot).getContactShapes('toe');
+l_foot_geometry_heel = robot.getBody(l_foot).getCollisionGeometry('heel');
+l_foot_geometry_toe = robot.getBody(l_foot).getCollisionGeometry('toe');
+r_foot_geometry_heel = robot.getBody(r_foot).getCollisionGeometry('heel');
+r_foot_geometry_toe = robot.getBody(r_foot).getCollisionGeometry('toe');
 l_foot_toe = [];
 l_foot_heel = [];
 r_foot_toe = [];
 r_foot_heel = [];
-for i = 1:length(l_foot_shapes_heel)
-  l_foot_heel = [l_foot_heel l_foot_shapes_heel{i}.getPoints];
+for i = 1:length(l_foot_geometry_heel)
+  l_foot_heel = [l_foot_heel l_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(l_foot_shapes_toe)
-  l_foot_toe = [l_foot_toe l_foot_shapes_toe{i}.getPoints];
+for i = 1:length(l_foot_geometry_toe)
+  l_foot_toe = [l_foot_toe l_foot_geometry_toe{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_heel)
-  r_foot_heel = [r_foot_heel r_foot_shapes_heel{i}.getPoints];
+for i = 1:length(r_foot_geometry_heel)
+  r_foot_heel = [r_foot_heel r_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_toe)
-  r_foot_toe = [r_foot_toe r_foot_shapes_toe{i}.getPoints];
+for i = 1:length(r_foot_geometry_toe)
+  r_foot_toe = [r_foot_toe r_foot_geometry_toe{i}.getPoints];
 end
 l_foot_bottom = [l_foot_toe l_foot_heel];
 r_foot_bottom = [r_foot_toe r_foot_heel];

solvers/trajectoryOptimization/dev/testComDynamicsFullKinematicsPlanner3.m

@@ -17,25 +17,25 @@
 
 l_foot = robot.findLinkInd('l_foot');
 r_foot = robot.findLinkInd('r_foot');
-l_foot_shapes_heel = robot.getBody(l_foot).getContactShapes('heel');
-l_foot_shapes_toe = robot.getBody(l_foot).getContactShapes('toe');
-r_foot_shapes_heel = robot.getBody(r_foot).getContactShapes('heel');
-r_foot_shapes_toe = robot.getBody(r_foot).getContactShapes('toe');
+l_foot_geometry_heel = robot.getBody(l_foot).getCollisionGeometry('heel');
+l_foot_geometry_toe = robot.getBody(l_foot).getCollisionGeometry('toe');
+r_foot_geometry_heel = robot.getBody(r_foot).getCollisionGeometry('heel');
+r_foot_geometry_toe = robot.getBody(r_foot).getCollisionGeometry('toe');
 l_foot_toe = [];
 l_foot_heel = [];
 r_foot_toe = [];
 r_foot_heel = [];
-for i = 1:length(l_foot_shapes_heel)
-  l_foot_heel = [l_foot_heel l_foot_shapes_heel{i}.getPoints];
+for i = 1:length(l_foot_geometry_heel)
+  l_foot_heel = [l_foot_heel l_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(l_foot_shapes_toe)
-  l_foot_toe = [l_foot_toe l_foot_shapes_toe{i}.getPoints];
+for i = 1:length(l_foot_geometry_toe)
+  l_foot_toe = [l_foot_toe l_foot_geometry_toe{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_heel)
-  r_foot_heel = [r_foot_heel r_foot_shapes_heel{i}.getPoints];
+for i = 1:length(r_foot_geometry_heel)
+  r_foot_heel = [r_foot_heel r_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_toe)
-  r_foot_toe = [r_foot_toe r_foot_shapes_toe{i}.getPoints];
+for i = 1:length(r_foot_geometry_toe)
+  r_foot_toe = [r_foot_toe r_foot_geometry_toe{i}.getPoints];
 end
 l_foot_bottom = [l_foot_toe l_foot_heel];
 r_foot_bottom = [r_foot_toe r_foot_heel];

solvers/trajectoryOptimization/dev/testComplementarityStaticContactConstraint.m

@@ -4,10 +4,10 @@
 nq = robot.getNumPositions();
 qstar = nomdata.xstar(1:robot.getNumPositions());
 l_foot = robot.findLinkInd('l_foot');
-nLPts = length(robot.getBody(l_foot).getContactShapes);
+nLPts = length(robot.getBody(l_foot).getCollisionGeometry);
 l_foot_pt = zeros(3,nLPts);
 for i=1:nLPts,
-  l_foot_pt(:,i) = robot.getBody(l_foot).getContactShapes{i}.getPoints;
+  l_foot_pt(:,i) = robot.getBody(l_foot).getCollisionGeometry{i}.getPoints;
 end
 num_pts = size(l_foot_pt,2);
 mu = 0.7;
@@ -51,4 +51,4 @@
 % kinsol1 = robot.doKinematics(q1,false,false);
 % kinsol2 = robot.doKinematics(q2,false,false);
 % [c,dc] = nlcon.eval(q1,q2,lambda,gamma,kinsol1,kinsol2);
-% end
+% end

solvers/trajectoryOptimization/dev/testJump.m

@@ -21,25 +21,25 @@ function testJump(mode)
 l_foot = robot.findLinkInd('l_foot');
 r_foot = robot.findLinkInd('r_foot');
 utorso = robot.findLinkInd('utorso');
-l_foot_shapes_heel = robot.getBody(l_foot).getContactShapes('heel');
-l_foot_shapes_toe = robot.getBody(l_foot).getContactShapes('toe');
-r_foot_shapes_heel = robot.getBody(r_foot).getContactShapes('heel');
-r_foot_shapes_toe = robot.getBody(r_foot).getContactShapes('toe');
+l_foot_geometry_heel = robot.getBody(l_foot).getCollisionGeometry('heel');
+l_foot_geometry_toe = robot.getBody(l_foot).getCollisionGeometry('toe');
+r_foot_geometry_heel = robot.getBody(r_foot).getCollisionGeometry('heel');
+r_foot_geometry_toe = robot.getBody(r_foot).getCollisionGeometry('toe');
 l_foot_toe = [];
 l_foot_heel = [];
 r_foot_toe = [];
 r_foot_heel = [];
-for i = 1:length(l_foot_shapes_heel)
-  l_foot_heel = [l_foot_heel l_foot_shapes_heel{i}.getPoints];
+for i = 1:length(l_foot_geometry_heel)
+  l_foot_heel = [l_foot_heel l_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(l_foot_shapes_toe)
-  l_foot_toe = [l_foot_toe l_foot_shapes_toe{i}.getPoints];
+for i = 1:length(l_foot_geometry_toe)
+  l_foot_toe = [l_foot_toe l_foot_geometry_toe{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_heel)
-  r_foot_heel = [r_foot_heel r_foot_shapes_heel{i}.getPoints];
+for i = 1:length(r_foot_geometry_heel)
+  r_foot_heel = [r_foot_heel r_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_toe)
-  r_foot_toe = [r_foot_toe r_foot_shapes_toe{i}.getPoints];
+for i = 1:length(r_foot_geometry_toe)
+  r_foot_toe = [r_foot_toe r_foot_geometry_toe{i}.getPoints];
 end
 l_foot_bottom = [l_foot_toe l_foot_heel];
 r_foot_bottom = [r_foot_toe r_foot_heel];

solvers/trajectoryOptimization/dev/testMonkeyBar.m

@@ -34,25 +34,25 @@ function testMonkeyBar(mode,ncp_tol)
 l_foot = robot.findLinkInd('l_foot');
 r_foot = robot.findLinkInd('r_foot');
 utorso = robot.findLinkInd('utorso');
-l_foot_shapes_heel = robot.getBody(l_foot).getContactShapes('heel');
-l_foot_shapes_toe = robot.getBody(l_foot).getContactShapes('toe');
-r_foot_shapes_heel = robot.getBody(r_foot).getContactShapes('heel');
-r_foot_shapes_toe = robot.getBody(r_foot).getContactShapes('toe');
+l_foot_geometry_heel = robot.getBody(l_foot).getCollisionGeometry('heel');
+l_foot_geometry_toe = robot.getBody(l_foot).getCollisionGeometry('toe');
+r_foot_geometry_heel = robot.getBody(r_foot).getCollisionGeometry('heel');
+r_foot_geometry_toe = robot.getBody(r_foot).getCollisionGeometry('toe');
 l_foot_toe = [];
 l_foot_heel = [];
 r_foot_toe = [];
 r_foot_heel = [];
-for i = 1:length(l_foot_shapes_heel)
-  l_foot_heel = [l_foot_heel l_foot_shapes_heel{i}.getPoints];
+for i = 1:length(l_foot_geometry_heel)
+  l_foot_heel = [l_foot_heel l_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(l_foot_shapes_toe)
-  l_foot_toe = [l_foot_toe l_foot_shapes_toe{i}.getPoints];
+for i = 1:length(l_foot_geometry_toe)
+  l_foot_toe = [l_foot_toe l_foot_geometry_toe{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_heel)
-  r_foot_heel = [r_foot_heel r_foot_shapes_heel{i}.getPoints];
+for i = 1:length(r_foot_geometry_heel)
+  r_foot_heel = [r_foot_heel r_foot_geometry_heel{i}.getPoints];
 end
-for i = 1:length(r_foot_shapes_toe)
-  r_foot_toe = [r_foot_toe r_foot_shapes_toe{i}.getPoints];
+for i = 1:length(r_foot_geometry_toe)
+  r_foot_toe = [r_foot_toe r_foot_geometry_toe{i}.getPoints];
 end
 l_foot_bottom = [l_foot_toe l_foot_heel];
 r_foot_bottom = [r_foot_toe r_foot_heel];

solvers/trajectoryOptimization/dev/testRigidBodyContactWrench.m

@@ -8,10 +8,10 @@
 
 display('Check FrictionConeWrench');
 l_foot = robot.findLinkInd('l_foot');
-nLPts = length(robot.getBody(l_foot).getContactShapes);
+nLPts = length(robot.getBody(l_foot).getCollisionGeometry);
 l_foot_pt = zeros(3,nLPts);
 for i=1:nLPts,
-  l_foot_pt(:,i) = robot.getBody(l_foot).getContactShapes{i}.getPoints;
+  l_foot_pt(:,i) = robot.getBody(l_foot).getCollisionGeometry{i}.getPoints;
 end
 num_pts = size(l_foot_pt,2);
 mu = 0.7;