Fix multiple calls to add**(Target|Constraint)

While the InverseKinematics library was designed to specify all the
targets and constraints **before** the  first call to solve,
apparently for some users it make sense to call again this method
between solve, to change some aspects of the constraints.

While this is not properly documented, this commit make sures
that if the user uses the methods in this way, everything works fine.

src/inverse-kinematics/src/InverseKinematics.cpp

@@ -375,6 +375,9 @@ namespace iDynTree {
         IK_PIMPL(m_pimpl)->m_comHullConstraint_yAxisOfPlaneInWorld = yAxisOfPlaneInWorld;
         IK_PIMPL(m_pimpl)->m_comHullConstraint_originOfPlaneInWorld = originOfPlaneInWorld;
 
+        // If this method is called again to reconfigure the constraint, the problem needs to be reinitialized
+        IK_PIMPL(m_pimpl)->m_problemInitialized = false;
+
         return true;
     }
 

src/inverse-kinematics/src/InverseKinematicsData.cpp

@@ -181,6 +181,10 @@ namespace kinematics {
 
         //add the constraint to the set
         std::pair<TransformMap::iterator, bool> result = m_constraints.insert(TransformMap::value_type(frameIndex, frameTransformConstraint));
+
+        // If this method is called again to reconfigure the constraint, the problem needs to be reinitialized
+        IK_PIMPL(m_pimpl)->m_problemInitialized = false;
+
         return result.second;
     }
 
@@ -195,6 +199,10 @@ namespace kinematics {
         if (result.second) {
             result.first->second.setTargetResolutionMode(m_defaultTargetResolutionMode);
         }
+
+        // If this method is called again to reconfigure the constraint, the problem needs to be reinitialized
+        IK_PIMPL(m_pimpl)->m_problemInitialized = false;
+
         return result.second;
     }