Added constraints | Fixing constraints in URDFParser
More constraint changes