Merge pull request #479 from traversaro/fixConsecutiveAddConstraint

InverseKinematics: Fix multiple calls to add**(Target|Constraint)
robotology · Sep 10, 2018 · 348803e · 348803e
2 parents 874d88d + e8be1c4
commit 348803e
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 0 deletions.
diff --git a/src/inverse-kinematics/src/InverseKinematics.cpp b/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;
     }
 

diff --git a/src/inverse-kinematics/src/InverseKinematicsData.cpp b/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;
     }