default frames for rigidbodymanipulators are no longer singletons.

justification from an email i sent out:

I've convinced myself that I should make a change to the default frame behavior for RigidBodyManipulators.  Currently, they are always instantiated as SingletonCoordinateFrames .  The mental model when I started this was that everything was going to live in the urdf, so one urdf -> one unique robot in drake.  We've drifted away from that substantially now by adding lots of modification abilities in matlab (starting with optional floating bases, to adding/removing collisions, and today i'm welding lots of joints).  So I now think it makes sense to make them good old fashion CoordinateFrames by default instead.  They are still handles... so everything in the code we have should still work (e.g. checking that frames are equal), but you simply won't be protected any more from generating two different frames with the same name and different meanings.  The most tangible result is that you will not be bothered by error messages which say "a singleton coordinate frame of this name already exists but with a different dimension" anymore.

I thought I'd run it by you since you are the primary consumers of the rigidbodymanipulator classes in drake.  Please squawk if you think I'm off here.  And I will test extensively before committing.

NOTE:  Atlas is immune here, since it (correctly) creates all of it's coordinate frames explicitly (and as singletoncoordinateframes).  This is how I would imagine it working - if you do have a particular robot that you care about, you can always make it have singleton frames.  But joe-shmoe-urdf will not.

UPDATE:  I think it was worse than that.  I now believe that i made the frames singleton because it was an easy way to overcome the problem of overwriting existing frames (and therefore losing transformations already established with other coordinate frames).  i've added that logic in now.

git-svn-id: https://svn.csail.mit.edu/locomotion/robotlib/trunk@6958 c9849af7-e679-4ec6-a44e-fc146a885bd3

.gitignore

@@ -88,3 +88,4 @@ lcmtypes/cpp
 lcmtypes/java
 lcmtypes/python
 build
+unitTest.log

examples/Atlas/test/singleJointParameterEstimation.m

@@ -0,0 +1,13 @@
+%function singleJointParameterEstimation
+
+r = RigidBodyManipulator('../urdf/atlas_minimal_contact.urdf');
+shoulder = findJointInd(r,'l_arm_usy');
+
+for i=1:getNumBodies(r)
+  if i~=shoulder
+    r = weldJoint(r,i);
+  end
+end
+
+r = compile(r);
+

systems/frames/CoordinateFrame.m

@@ -96,7 +96,14 @@ function disp(obj)
         fprintf(1,'  %s\n',obj.coordinates{i});
       end
     end
-
+
+    function tf = isequal_modulo_transforms(a,b)
+      tf = isequal(a.name,b.name) && ...
+        isequal(a.dim,b.dim) && ...
+        isequal(a.coordinates,b.coordinates) && ...
+        isequal(a.prefix,b.prefix);
+    end
+
     function s = getSym(obj)
       for i=1:length(obj.dim)
         s(1) = sym(obj.coordinates{i},'real');

systems/frames/MultiCoordinateFrame.m

@@ -109,6 +109,16 @@ function display(obj)
       end
     end
 
+    function tf = isequal_modulo_transforms(a,b)
+      tf = isequal_modulo_transforms@CoordinateFrame(a,b) && ...
+        isequal(a.frame_id,b.frame_id) && ...
+        isequal(a.coord_ids,b.coord_ids) && ...
+        isequal(size(a.frame),size(b.frame));
+      for i=1:length(a.frame)
+        tf = tf && isequal_modulo_transforms(a.frame{i},b.frame{i});
+      end
+    end
+
     function tf = findTransform(obj,target,options)
       % There are two ways to get a transform from this multiframe to
       % another frame.  One is if a transform exists directly from the

systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m

@@ -427,14 +427,17 @@
       model = model.setNumOutputs(2*num_dof);
 
       if getNumInputs(model)>0
-        model = setInputFrame(model,constructInputFrame(model));
+        inputframe = constructInputFrame(model);
+        if ~isequal_modulo_transforms(inputframe,getInputFrame(model)) % let the previous handle stay valid if possible
+          model = setInputFrame(model,inputframe);
+        end
       end
 
       if getNumStates(model)>0
         stateframe = constructStateFrame(model);
-        model = setStateFrame(model,stateframe);
-      else
-        stateframe = getStateFrame(model);  % needed for output frame logic below
+        if ~isequal_modulo_transforms(stateframe,getStateFrame(model)) % let the previous handle stay valid if possible
+          model = setStateFrame(model,stateframe);
+        end
       end
 
       if length(model.sensor)>0
@@ -445,11 +448,13 @@
           feedthrough = feedthrough || model.sensor{i}.isDirectFeedthrough;
         end
         fr = MultiCoordinateFrame.constructFrame(outframe);
-        model = setNumOutputs(model,fr.dim);
-        model = setOutputFrame(model,fr);
+        if ~isequal_modulo_transforms(fr,getOutputFrame(model)) % let the previous handle stay valid if possible
+          model = setNumOutputs(model,fr.dim);
+          model = setOutputFrame(model,fr);
+        end
         model = setDirectFeedthrough(model,feedthrough);
       else
-        model = setOutputFrame(model,stateframe);  % output = state
+        model = setOutputFrame(model,getStateFrame(model));  % output = state
         model = setDirectFeedthrough(model,false);
       end
 
@@ -544,11 +549,25 @@
     end
 
     function model = setBody(model,body_ind,body)
-      typecheck(body_ind,'double');
+      typecheck(body_ind,'numeric');
       typecheck(body,'RigidBody');
       model.body(body_ind) = body;
       model.dirty = true;
     end
+
+    function model = weldJoint(model,body_ind_or_joint_name,robot)
+      if ischar(body_ind_or_joint_name)
+        if nargin>2
+          body_ind_or_joint_name = findJointInd(model,body_ind_or_joint_name,robot)
+        else
+          body_ind_or_joint_name = findJointINd(model,body_ind_or_joint_name);
+        end
+      end
+
+      typecheck(body_ind_or_joint_name,'numeric');
+      model.body(body_ind_or_joint_name).pitch = nan;
+      model.dirty = true;
+    end
 
     function body_ind = findJointInd(model,jointname,robot)
       % @param robot can be the robot number or the name of a robot
@@ -643,7 +662,7 @@ function drawKinematicTree(model)
     end
 
     function fr = constructCOMFrame(model)
-      fr = SingletonCoordinateFrame([model.name,'COM'],3,'m',{'com_x','com_y','com_z'});
+      fr = CoordinateFrame([model.name,'COM'],3,'m',{'com_x','com_y','com_z'});
 
       return; 
 
@@ -931,7 +950,7 @@ function checkDirty(obj)
           end
         end
         coordinates = vertcat(joints,cellfun(@(a) [a,'dot'],joints,'UniformOutput',false));
-        fr{i} = SingletonCoordinateFrame([model.name{i},'State'],length(coordinates),'x',coordinates);
+        fr{i} = CoordinateFrame([model.name{i},'State'],length(coordinates),'x',coordinates);
       end
 %      frame_dims=[model.body(arrayfun(@(a) ~isempty(a.parent),model.body)).robotnum];
       frame_dims=[frame_dims,frame_dims];
@@ -955,12 +974,15 @@ function checkDirty(obj)
       for i=1:length(model.name)
         robot_inputs = [inputparents.robotnum]==i;
         coordinates = {inputnames{robot_inputs}}';
-        fr{i}=SingletonCoordinateFrame([model.name{i},'Input'],sum(robot_inputs),'u',coordinates);
+        fr{i}=CoordinateFrame([model.name{i},'Input'],sum(robot_inputs),'u',coordinates);
       end
       frame_dims = [inputparents.robotnum];
       fr = MultiCoordinateFrame.constructFrame(fr,frame_dims,true);
     end
 
+    function fr = constructOutputFrame(model)
+    end    
+
     function [model,dof] = extractFeatherstone(model)
 %      m=struct('NB',{},'parent',{},'jcode',{},'Xtree',{},'I',{});
       dof=0;inds=[];
@@ -1118,7 +1140,7 @@ function checkDirty(obj)
           end
         end
 
-        % error on actuators
+        % remove actuators
         if (~isempty(model.actuator) && any([model.actuator.joint] == i))
           model.actuator(find([model.actuator.joint]==i))=[];
           % actuators could be attached to fixed joints, because I

systems/plants/TimeSteppingRigidBodyManipulator.m

@@ -118,8 +118,10 @@ function checkDirty(obj)
           feedthrough = feedthrough || model.sensor{i}.isDirectFeedthrough;
         end
         fr = MultiCoordinateFrame.constructFrame(outframe);
-        model = setNumOutputs(model,fr.dim);
-        model = setOutputFrame(model,fr);
+        if ~isequal_modulo_transforms(fr,getOutputFrame(model))
+          model = setNumOutputs(model,fr.dim);
+          model = setOutputFrame(model,fr);
+        end
         model = setDirectFeedthrough(model,feedthrough);
       else
         model = setNumOutputs(model,getNumOutputs(model.manip));