Include std map to BodyNode.h
Implementing recursive algorithm for calculating inverse mass matrix (not done yet)