diff --git a/src/inverse-kinematics/src/InverseKinematics.cpp b/src/inverse-kinematics/src/InverseKinematics.cpp index e93f25d402f..7266c62a328 100644 --- 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 index 71b26fe9a95..3132e653c20 100644 --- 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 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; }