[2016.09.22]
Done
put Inertia and Mass data in links
fixed urdf reader so we can get every information from urdf
find another urdf reader which is more easy to use but need to check if it is applicable to our assemble code
TODO
needs to figure out which urdf reader we will use to finish assemble quickly
To build collision models for dynamic behavior and to finalize the parser program.