Update the utils directory

Added utils and optimized some old ones for speed. Increased inverse
kinematics speed considerably.
Update of the Kinematics files.

Updates the RoboHAZMAT kinematics and adds the kinematics.m file for the
Mechatronic Arm.
Allows for multiple Robots

Generalizes the class to allow multiple robots to be created.
Dramatic increase to optimization

Speeds up the optimization for the inverse kinematics to almost
real-time.
Serial communication expanded

Expands serial communication to include communication with the Arduino
used to control the Mechatronic Arm. Also generalizes COM port setup in
one place.