From e8be1c44bded9b383250b00c6ee56328199786f9 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 10 Sep 2018 11:43:50 +0200 Subject: [PATCH] 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 | 3 +++ src/inverse-kinematics/src/InverseKinematicsData.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) 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; }