<commit_before>//
// Copyright (c) 2014 CNRS
// Authors: <NAME>
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <limits>
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <hpp/model/configuration.hh>
#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/constraints/svd.hh>
#include <hpp/constraints/macros.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/core/explicit-numerical-constraint.hh>

// #define SVD_THRESHOLD Eigen::NumTraits<value_type>::dummy_precision()
#define SVD_THRESHOLD 1e-5

namespace hpp {
  namespace core {
    namespace {
      HPP_DEFINE_TIMECOUNTER (projection);
      HPP_DEFINE_TIMECOUNTER (optimize);
    }

    HPP_DEFINE_REASON_FAILURE (REASON_MAX_ITER, "Max Iterations reached");
    HPP_DEFINE_REASON_FAILURE (REASON_ERROR_INCREASED, "Error increased");

    //using boost::fusion::result_of::at;
    bool operator< (const LockedJointPtr_t& l1, const LockedJointPtr_t& l2)
    {
      return l1->rankInVelocity () < l2->rankInVelocity ();
    }

    ConfigProjectorPtr_t ConfigProjector::create (const DevicePtr_t& robot,
						  const std::string& name,
						  value_type errorThreshold,
						  size_type maxIterations)
    {
      ConfigProjector* ptr = new ConfigProjector (robot, name, errorThreshold,
						  maxIterations);
      ConfigProjectorPtr_t shPtr (ptr);
      ptr->init (shPtr);
      return shPtr;
    }

    ConfigProjectorPtr_t
    ConfigProjector::createCopy (const ConfigProjectorPtr_t cp)
    {
      ConfigProjector* ptr = new ConfigProjector (*cp);
      ConfigProjectorPtr_t shPtr (ptr);
      ptr->init (shPtr);
      return shPtr;
    }

    ConfigProjector::ConfigProjector (const DevicePtr_t& robot,
				      const std::string& name,
				      value_type errorThreshold,
				      size_type maxIterations) :
      Constraint (name), robot_ (robot), functions_ (),
      passiveDofs_ (), lockedJoints_ (),
      squareErrorThreshold_ (errorThreshold * errorThreshold),
      maxIterations_ (maxIterations), rhsReducedSize_ (0),
      lastIsOptional_ (false),
      toMinusFrom_ (robot->numberDof ()),
      projMinusFrom_ (robot->numberDof ()),
      dq_ (robot->numberDof ()),
      dqSmall_ (robot->numberDof ()),
      nbNonLockedDofs_ (robot_->numberDof ()),
      nbLockedDofs_ (0),
      squareNorm_(0), explicitComputation_ (false), weak_ (),
      statistics_ ("ConfigProjector " + name)
    {
      dq_.setZero ();
      stack_.push_back (PriorityStack (3,nbNonLockedDofs_)); /// First and last
    }

    ConfigProjector::ConfigProjector (const ConfigProjector& cp) :
      Constraint (cp), robot_ (cp.robot_), stack_ (cp.stack_),
      functions_ (cp.functions_),
      passiveDofs_ (cp.passiveDofs_), lockedJoints_ (),
      intervals_ (cp.intervals_),
      squareErrorThreshold_ (cp.squareErrorThreshold_),
      maxIterations_ (cp.maxIterations_),
      rightHandSide_ (cp.rightHandSide_),
      rhsReducedSize_ (cp.rhsReducedSize_),
      lastIsOptional_ (cp.lastIsOptional_),
      value_ (cp.value_.size ()),
      reducedJacobian_ (cp.reducedJacobian_.rows (),
			cp.reducedJacobian_.cols ()),
      svd_ (cp.reducedJacobian_.rows (), cp.reducedJacobian_.cols (),
          Eigen::ComputeThinU | Eigen::ComputeThinV),
      reducedProjector_ (cp.reducedProjector_.rows (),
			 cp.reducedProjector_.cols ()),
      toMinusFrom_ (cp.toMinusFrom_.size ()),
      projMinusFrom_ (cp.projMinusFrom_.size ()),
      dq_ (cp.dq_.size ()), dqSmall_ (cp.dqSmall_.size ()),
      nbNonLockedDofs_ (cp.nbNonLockedDofs_), nbLockedDofs_ (cp.nbLockedDofs_),
      squareNorm_ (cp.squareNorm_),
      explicitComputation_ (cp.explicitComputation_), weak_ (),
      statistics_ (cp.statistics_)
    {
      dq_.setZero ();
      for (LockedJoints_t::const_iterator it = cp.lockedJoints_.begin ();
	   it != cp.lockedJoints_.end (); ++it) {
	lockedJoints_.push_back (HPP_STATIC_PTR_CAST (LockedJoint,
						      (*it)->copy ()));
      }
    }

    ConstraintPtr_t ConfigProjector::copy () const
    {
      return createCopy (weak_.lock ());
    }

    ConfigProjector::PriorityStack::PriorityStack (std::size_t level,
        std::size_t cols) :
      level_ (level), outputSize_ (0), cols_ (cols),
      svd_ (outputSize_,cols,Eigen::ComputeThinU | Eigen::ComputeThinV),
      PK_ (cols, cols)
    {
      svd_.setThreshold (SVD_THRESHOLD);
    }

    void ConfigProjector::PriorityStack::add (
        const NumericalConstraintPtr_t& nm, const SizeIntervals_t& passiveDofs)
    {
      functions_.push_back (nm);
      passiveDofs_.push_back (passiveDofs);
      outputSize_ += nm->function().outputSize ();
      svd_ = SVD_t (outputSize_, cols_,
          Eigen::ComputeThinU | Eigen::ComputeThinV);
      svd_.setThreshold (SVD_THRESHOLD);
    }

    void ConfigProjector::PriorityStack::nbNonLockedDofs
      (const std::size_t cols)
    {
      cols_ = cols;
      svd_ = SVD_t (outputSize_, cols_,
          Eigen::ComputeThinU | Eigen::ComputeThinV);
      svd_.setThreshold (SVD_THRESHOLD);
      PK_.resize (cols_, cols_);
    }

    void ConfigProjector::add (const NumericalConstraintPtr_t& nm,
        const SizeIntervals_t& passiveDofs,
        const std::size_t priority)
    {
      if (HPP_DYNAMIC_PTR_CAST (ExplicitNumericalConstraint, nm)) {
	explicitFunctions_.push_back (functions_.size ());
      }
      functions_.push_back (nm);
      passiveDofs_.push_back (passiveDofs);
      rhsReducedSize_ += nm->rhsSize ();
      if (stack_.size () > 0)
        stack_.back ().level_ = 1; // General case
      for (std::size_t i = stack_.size (); i < priority + 1; ++i) {
        stack_.push_back (PriorityStack (1, nbNonLockedDofs_)); // Middle
      }
      if (priority > 0) { // There are more than 2 levels
        stack_.front ().level_ = 0; // First
        stack_.back  ().level_ = 2; // Last
      }
      stack_[priority].add (nm, passiveDofs);
      // TODO: no need to recompute intervals.
      computeIntervals ();
      resize ();
      updateExplicitComputation ();
    }

    void ConfigProjector::computeIntervals ()
    {
      intervals_.clear ();
      nbLockedDofs_ = 0;
      std::pair < size_type, size_type > interval;
      std::size_t latestIndex = 0;
      size_type size;
      lockedJoints_.sort ();
      // temporarily add an element at the end of the list.
      lockedJoints_.push_back (LockedJoint::create (robot_));
      for (LockedJoints_t::const_iterator itLocked = lockedJoints_.begin ();
	   itLocked != lockedJoints_.end (); ++itLocked) {
    std::size_t index = (*itLocked)->rankInVelocity ();
	nbLockedDofs_ += (*itLocked)->numberDof ();
	hppDout (info, "number locked dof " << (*itLocked)->numberDof ());
	size = (index - latestIndex);
	if (size > 0) {
	  interval.first = latestIndex;<commit_msg>Fix possible issues in ConfigProjector<commit_after>//
// Copyright (c) 2014 CNRS
// Authors: <NAME>
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <limits>
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <hpp/model/configuration.hh>
#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/constraints/svd.hh>
#include <hpp/constraints/macros.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/core/explicit-numerical-constraint.hh>

// #define SVD_THRESHOLD Eigen::NumTraits<value_type>::dummy_precision()
#define SVD_THRESHOLD 1e-8

namespace hpp {
  namespace core {
    namespace {
      HPP_DEFINE_TIMECOUNTER (projection);
      HPP_DEFINE_TIMECOUNTER (optimize);
    }

    HPP_DEFINE_REASON_FAILURE (REASON_MAX_ITER, "Max Iterations reached");
    HPP_DEFINE_REASON_FAILURE (REASON_ERROR_INCREASED, "Error increased");

    //using boost::fusion::result_of::at;
    bool operator< (const LockedJointPtr_t& l1, const LockedJointPtr_t& l2)
    {
      return l1->rankInVelocity () < l2->rankInVelocity ();
    }

    ConfigProjectorPtr_t ConfigProjector::create (const DevicePtr_t& robot,
						  const std::string& name,
						  value_type errorThreshold,
						  size_type maxIterations)
    {
      ConfigProjector* ptr = new ConfigProjector (robot, name, errorThreshold,
						  maxIterations);
      ConfigProjectorPtr_t shPtr (ptr);
      ptr->init (shPtr);
      return shPtr;
    }

    ConfigProjectorPtr_t
    ConfigProjector::createCopy (const ConfigProjectorPtr_t cp)
    {
      ConfigProjector* ptr = new ConfigProjector (*cp);
      ConfigProjectorPtr_t shPtr (ptr);
      ptr->init (shPtr);
      return shPtr;
    }

    ConfigProjector::ConfigProjector (const DevicePtr_t& robot,
				      const std::string& name,
				      value_type errorThreshold,
				      size_type maxIterations) :
      Constraint (name), robot_ (robot), functions_ (),
      passiveDofs_ (), lockedJoints_ (),
      squareErrorThreshold_ (errorThreshold * errorThreshold),
      maxIterations_ (maxIterations), rhsReducedSize_ (0),
      lastIsOptional_ (false),
      toMinusFrom_ (robot->numberDof ()),
      projMinusFrom_ (robot->numberDof ()),
      dq_ (robot->numberDof ()),
      dqSmall_ (robot->numberDof ()),
      nbNonLockedDofs_ (robot_->numberDof ()),
      nbLockedDofs_ (0),
      squareNorm_(0), explicitComputation_ (false), weak_ (),
      statistics_ ("ConfigProjector " + name)
    {
      dq_.setZero ();
      stack_.push_back (PriorityStack (3,nbNonLockedDofs_)); /// First and last
    }

    ConfigProjector::ConfigProjector (const ConfigProjector& cp) :
      Constraint (cp), robot_ (cp.robot_), stack_ (cp.stack_),
      functions_ (cp.functions_),
      passiveDofs_ (cp.passiveDofs_), lockedJoints_ (),
      intervals_ (cp.intervals_),
      squareErrorThreshold_ (cp.squareErrorThreshold_),
      maxIterations_ (cp.maxIterations_),
      rightHandSide_ (cp.rightHandSide_),
      rhsReducedSize_ (cp.rhsReducedSize_),
      lastIsOptional_ (cp.lastIsOptional_),
      value_ (cp.value_.size ()),
      reducedJacobian_ (cp.reducedJacobian_.rows (),
			cp.reducedJacobian_.cols ()),
      svd_ (cp.reducedJacobian_.rows (), cp.reducedJacobian_.cols (),
          Eigen::ComputeThinU | Eigen::ComputeThinV),
      reducedProjector_ (cp.reducedProjector_.rows (),
			 cp.reducedProjector_.cols ()),
      toMinusFrom_ (cp.toMinusFrom_.size ()),
      projMinusFrom_ (cp.projMinusFrom_.size ()),
      dq_ (cp.dq_.size ()), dqSmall_ (cp.dqSmall_.size ()),
      nbNonLockedDofs_ (cp.nbNonLockedDofs_), nbLockedDofs_ (cp.nbLockedDofs_),
      squareNorm_ (cp.squareNorm_),
      explicitComputation_ (cp.explicitComputation_), weak_ (),
      statistics_ (cp.statistics_)
    {
      dq_.setZero ();
      for (LockedJoints_t::const_iterator it = cp.lockedJoints_.begin ();
	   it != cp.lockedJoints_.end (); ++it) {
	lockedJoints_.push_back (HPP_STATIC_PTR_CAST (LockedJoint,
						      (*it)->copy ()));
      }
    }

    ConstraintPtr_t ConfigProjector::copy () const
    {
      return createCopy (weak_.lock ());
    }

    ConfigProjector::PriorityStack::PriorityStack (std::size_t level,
        std::size_t cols) :
      level_ (level), outputSize_ (0), cols_ (cols),
      svd_ (outputSize_,cols,Eigen::ComputeThinU | Eigen::ComputeThinV),
      PK_ (cols, cols)
    {
      svd_.setThreshold (SVD_THRESHOLD);
    }

    void ConfigProjector::PriorityStack::add (
        const NumericalConstraintPtr_t& nm, const SizeIntervals_t& passiveDofs)
    {
      functions_.push_back (nm);
      passiveDofs_.push_back (passiveDofs);
      outputSize_ += nm->function().outputSize ();
      svd_ = SVD_t (outputSize_, cols_,
          Eigen::ComputeThinU | Eigen::ComputeThinV);
      svd_.setThreshold (SVD_THRESHOLD);
    }

    void ConfigProjector::PriorityStack::nbNonLockedDofs
      (const std::size_t cols)
    {
      cols_ = cols;
      svd_ = SVD_t (outputSize_, cols_,
          Eigen::ComputeThinU | Eigen::ComputeThinV);
      svd_.setThreshold (SVD_THRESHOLD);
      PK_.resize (cols_, cols_);
    }

    void ConfigProjector::add (const NumericalConstraintPtr_t& nm,
        const SizeIntervals_t& passiveDofs,
        const std::size_t priority)
    {
      if (HPP_DYNAMIC_PTR_CAST (ExplicitNumericalConstraint, nm)) {
	explicitFunctions_.push_back (functions_.size ());
      }
      functions_.push_back (nm);
      passiveDofs_.push_back (passiveDofs);
      rhsReducedSize_ += nm->rhsSize ();
      if (priority >= stack_.size ()) { // If we must add a priority level
        stack_.front ().level_ = 0; // become (or stay) First
        stack_.back ().level_ = 1; // becomes Middle
        for (std::size_t i = stack_.size (); i < priority; ++i) {
          stack_.push_back (PriorityStack (1, nbNonLockedDofs_)); // Middle
        }
        stack_.push_back (PriorityStack (2, nbNonLockedDofs_)); // Last
      }
      if (priority > 0) { // There are more than 2 levels
        assert (stack_.front ().level_ == 0);
        assert (stack_.back ().level_ == 2);
        // stack_.front ().level_ = 0; // First
        // stack_.back  ().level_ = 2; // Last
      } else {
        assert (stack_.front ().level_ == 3);
      }
      stack_[priority].add (nm, passiveDofs);
      // TODO: no need to recompute intervals.
      computeIntervals ();
      resize ();
      updateExplicitComputation ();
    }

    void ConfigProjector::computeIntervals ()
    {
      intervals_.clear ();
      nbLockedDofs_ = 0;
      std::pair < size_type, size_type > interval;
      std::size_t latestIndex = 0;
      size_type size;
      lockedJoints_.sort ();
      // temporarily add an element at the end of the list.
      lockedJoints_.push_back (LockedJoint::create (robot_));
      for (LockedJoints_t::const_iterator itLocked = lockedJoints_.begin ();
	   itLocked != lockedJoints_.end (); ++itLocked) {
    std::size_t index = (*itLocked)->rankInVelocity ();
	nbLockedDofs_ += (*itLocked)->numberDof ();
	hppDout (info, "number locked dof " << (*itLocked)->numberDof ());
	size = (index - latestIndex);
	if (size > 0) {
	  interval.first = latestIndex;
{'fraction_non_alphanumeric': 0.12188508715300193, 'fraction_numerical': 0.0036797934151065206, 'mean_word_length': 3.192422192151556, 'pattern_counts': {'":': 0, '<': 47, '<?xml version=': 0, '>': 73, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '203270', 'n_tokens_mistral': 5230, 'n_tokens_neox': 4979, 'n_words': 1104}