Merge pull request #1 from gbledt/master

IMU added to simulation
Update and rename README.txt to README.md
Update README.md
Update README.md
Merge pull request #2 from gbledt/master

Optimization update, Serial communication functionality, Mechatronic Arm additions
Merge pull request #3 from gbledt/master

Interactive simulation update and documentation
Update README.md
Merge pull request #4 from gbledt/master

Update and rename README.txt to README.md
Merge pull request #5 from gbledt/master

Small updates to figure plotting
Merge pull request #1 from RoboHAZMAT/master

Sync with master