Fix IK test

This commit is a fix to some functions used in IK
  1. In CMakeList, rather than using add_mex to add libraries or executables, use add_library or add_executable instead. add_mex introduces the `liblast` problem discussed in https://github.com/RobotLocomotion/drake/issues/150. Now even without fixing the liblast problem, those C++ executable tests can run.
  2. Fix some bugs in `RigidBodyConstraint.cpp` and `inverseKinBackend.cpp`. They were detected by compiling in debug mode. I can not believe those bugs survived so many tests.
  3. Add more tests for IK in C++.
  4. Fix some bugs for evaluating RigidBodyConstraint at a time outside of tspan. Add the test.
  5. I put `add_test` in systems/plants/test/CMakeLists.txt for the C++ executable tests, but it seems unitTest does not crawl into pod-build/bin folder
  6.  The way I link to MATLAB libraries in systems/plants/testCMakeLists.txt, line 20 might not work on other systems. Should test it on a Mac or Windows 32bit machine before merging it in.
  7. In URDFRigidBodyManipulator, add robotnum to each body.