Add sub graph functionality to scene graph

[mpowelson]

Tesseract already supports adding scene graphs to other scene graphs. Additional enhancements be done.

• [ ] Add sub graph functionality to scene graph
• [ ] Add sub graph solver functionality to tesseract_environment. The idea behind this to mimic actual hardware in the environment.

Taken from #66