diff --git a/include/hpp/constraints/configuration-constraint.hh b/include/hpp/constraints/configuration-constraint.hh index 244452a1..cdc6892d 100644 --- a/include/hpp/constraints/configuration-constraint.hh +++ b/include/hpp/constraints/configuration-constraint.hh @@ -72,6 +72,7 @@ namespace hpp { vector_t weights_; EigenBoolVector_t mask_; mutable vector_t diff_; + mutable Eigen::Matrix J_; }; // class ComBetweenFeet } // namespace constraints } // namespace hpp diff --git a/src/configuration-constraint.cc b/src/configuration-constraint.cc index 7b527ac9..89634b71 100644 --- a/src/configuration-constraint.cc +++ b/src/configuration-constraint.cc @@ -80,13 +80,13 @@ namespace hpp { matrix_t unused; LiegroupConstElementRef a (argument, goal_.space()); - diff_ = goal_ - a; + J_ = (goal_ - a).transpose(); // Apply jacobian of the difference on the right. - goal_.space()->Jdifference (argument, goal_.vector(), diff_.transpose(), unused); + goal_.space()->Jdifference (argument, goal_.vector(), J_, unused); jacobian.leftCols (robot_->numberDof ()).noalias() = - weights_.cwiseProduct(diff_).transpose (); + J_.cwiseProduct(weights_.transpose()); } } // namespace constraints } // namespace hpp