In urdf parser, handle failure to insert an already existing frame.

  - in function addJointAndBody, when add frame with name joint_name, if the
    joint already exists, method Model::addJointFrame returns -1. This event was
    not detected leading to undefined behavior.
  - this commit detects this error and throws an exception.