Include all the commits I made back from Nov 16th.

This topic branch add the following features to drake
1. Add a new type of constraint, the SingleTimeLinearPostureConstraint. This constraint specifies lb<=A*q<=ub. Also inverseKin,inverseKinPointwise,inverseKinTraj support this constraint type, but not the approximateIK.
2. All IK functions now have a separate C++ interface, so they do not need to be called through MATLAB. Also they have the C++ test files, built in pod-build/bin
3. All constraints can be constructed from the same function, constructRigidBodyConstraint. Each non-abstract constraint has a unique constraint type.
4. I add constness to the methods in RigidBodyConstraint.
5. URDFRigidBodyManipulator now add joint limits and masses to its field. But URDFRigidBodyManipular and RigidBodyManipulator in MATLAB are not exactly the same for the same URDF file. The former one sort the link and joint names, so the link/joint index are different from the MATLAB counterpart.
6. Add findLinkInd method in C++ RigidBodyManipulator. But I am not handling the regular expressions, as does in the MATLAB counterpart.
7. Changed the definition of 'tol' in the WorldQuatConstraint. Previously it was sin(theta/2)^2, where theta is the allowable angle to rotate. This definition is quite unnatural. Now tol = theta.

doc/derivations/floatingBaseTrajOpt.tex

@@ -0,0 +1,39 @@
+\documentclass{article}
+\usepackage{amsmath,verbatim}
+\begin{document}
+Suppose we are doing trajectory optimization for the robot with floating base model
+\begin{align}
+\min_{q,\dot{q},\ddot{q},u,\lambda} \int^T_0 u^TRu\\
+\text{s.t } \begin{bmatrix} H_f\\H_n\end{bmatrix}\ddot{q}+\begin{bmatrix}C_f\\C_n\end{bmatrix}=\begin{bmatrix}0\\I\end{bmatrix}u+\begin{bmatrix}J_f^T\\J_n^T\end{bmatrix}\lambda
+\end{align}
+where the subscript $f,n$ denote \textsl{floating base} and \textsl{non-floating base} respectively.
+
+We notice that for the non-floating base part, there is always a feasible control $u$ for any $\ddot{q},\dot{q},q,\lambda$ if we ignore the joint limits. So we can consider to drop that part of the constraint, and only focus on the floating base part.
+\begin{align}
+\min_{q,\dot{q},\ddot{q},u,\lambda} \int^T_0 u^TRu\\
+\text{s.t } H_f\ddot{q}+C_f=J_f^T\lambda
+\end{align}
+We notice that $u=H_n\ddot{q}+C_n-J_n^T\lambda$. So the quadratic term in the objective function $u^TRu$ is just a quadratic function of $\ddot{q},\dot{q},q,\lambda$
+\begin{align}
+\min_{q,\dot{q},\ddot{q},\lambda} \int^T_0 |H_n\ddot{q}+C_n-J_n^T\lambda|^2_R\\
+\text{s.t } H_f\ddot{q}+C_f=J_f^T\lambda
+\end{align}
+Still we have $\ddot{q}$ in the decision variable. But we notice that the equality constraint is affine in $\ddot{q}$ and the cost is quadratic in $\ddot{q}$. So the optimal value can be represented as a function of other decision variables $\dot{q},q,\lambda$ only.
+\begin{align}
+\min_{q,\dot{q},\lambda} \int^T_0 \begin{bmatrix}q\\\dot{q}\\\lambda\end{bmatrix}^TM(q,\dot{q},\lambda)\begin{bmatrix}q\\\dot{q}\\\lambda\end{bmatrix}+N(q,\dot{q},\lambda)^T\begin{bmatrix}q\\\dot{q}\\\lambda\end{bmatrix}\\
+\text{s.t } H_f\ddot{q}+C_f=J_f^T\lambda
+\end{align}
+Where $M,N$ are matrices depends on $q,\dot{q},\lambda$
+
+To transcribe the differential dynamics constraint to algebraic constraint, we consider
+\begin{equation}
+H_f\ddot{q}=\frac{dH_f\dot{q}}{dt}-\dot{H}_f\dot{q}
+\end{equation}
+And thus
+\begin{equation}
+\frac{dH_f\dot{q}}{dt}=J_f^T\lambda-C_f-\dot{H}_f\dot{q}
+\end{equation}
+We can then assume the term $H_f\dot{q}$ is a cubic Hermite polynomial, and transcribe it to an algebraic constraint that only depends on $q,\dot{q},\lambda$.
+
+So we end up with an optimization problem with only $q,\dot{q},\lambda$ as decision variables, and 6 constraints only.
+\end{document}

rmpath_drake.m

@@ -11,6 +11,7 @@
 rmpath(fullfile(conf.root,'systems','plants'));
 rmpath(fullfile(conf.root,'systems','plants','affordance'));
 rmpath(fullfile(conf.root,'systems','plants','collision'));
+rmpath(fullfile(conf.root,'systems','plants','constraint'));
 rmpath(fullfile(conf.root,'systems','controllers'));
 rmpath(fullfile(conf.root,'systems','trajectories'));
 rmpath(fullfile(conf.root,'systems','frames'));

systems/plants/@RigidBodyManipulator/approximateIK.m

@@ -24,7 +24,7 @@
       error('The input has to be a RigidBodyConstraint object');
     end
   end
-  [q,info] = approximateIKmex(obj.mex_model_ptr,q_seed,q_nom,constraint_ptr_cell{:},ikoptions);
+  [q,info] = approximateIKmex(obj.mex_model_ptr,q_seed,q_nom,constraint_ptr_cell{:},ikoptions.mex_ptr);
 else
 t = [];
 kc_cell = {};
@@ -71,7 +71,7 @@
 params.presolve = 0;
 params.bariterlimit = 20;
 params.barhomogeneous = 0;
-params.barconvtol = 1e-4;
+params.barconvtol = 0.005;
 
 result = gurobi(model,params);
 info = ~strcmp(result.status,'OPTIMAL');

systems/plants/@RigidBodyManipulator/inverseKin.m

@@ -37,9 +37,35 @@
 
 % note: keeping typecheck/sizecheck to a minimum because this might have to
 % run inside a dynamical system (so should be fast)
-if(nargout == 3)
-  [q,info,infeasible_constraint]= inverseKinBackend(obj,1,[],q_seed,q_nom,varargin{:});
+use_mex = false;
+if(isa(varargin{end},'IKoptions'))
+  ikoptions = varargin{end};
+  ikoptions_mex = ikoptions.mex_ptr;
+  varargin = varargin(1:end-1);
+elseif(isa(varargin{end},'DrakeMexPointer'))
+  if(strcmp(varargin{end}.name,'IKoptions'))
+    ikoptions_mex = varargin{end};
+    varargin = varargin(1:end-1);
+    use_mex = true;
+  end
 else
-  [q,info]= inverseKinBackend(obj,1,[],q_seed,q_nom,varargin{:});
+  ikoptions = IKoptions(obj);
+  ikoptions_mex = ikoptions.mex_ptr;
+end
+constraint_mex = cell(1,length(varargin));
+for i = 1:length(varargin)
+  if(isa(varargin{i},'RigidBodyConstraint'))
+    constraint_mex{i} = varargin{i}.mex_ptr;
+  elseif(isa(varargin{i},'DrakeConstraintMexPointer'))
+    constraint_mex{i} = varargin{i};
+    use_mex = true;
+  else
+    error('Drake:inverseKin: The input should be a RigidBodyConstraint or a pointer to RigidBodyConstraint');
+  end
+end
+if(use_mex || ikoptions.use_mex )
+  [q,info,infeasible_constraint] = inverseKinmex(obj.getMexModelPtr,q_seed,q_nom,constraint_mex{:},ikoptions_mex);
+else
+  [q,info,infeasible_constraint]= inverseKinBackend(obj,1,[],q_seed,q_nom,varargin{:});
 end
 end