Add class for manipulation
Add parameter to Robot class to avoid reloading urdf file.