<commit_before>#include "needle_problem_helper.hpp"
#include "utils.hpp"
#include "costs.hpp"
#include "constraints.hpp"

namespace Needle {
  template<typename T, size_t N>
  T* end(T (&ra)[N]) {
    return ra + N;
  } 

  void NeedleProblemHelper::AddRotationCost(OptProb& prob, NeedleProblemInstancePtr pi) {
    switch (rotation_cost) {
      case UseRotationQuadraticCost: {
        prob.addCost(CostPtr(new RotationQuadraticCost(pi->phivars.col(0), coeff_rotation, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      case UseRotationL1Cost: {
        prob.addCost(CostPtr(new RotationL1Cost(pi->phivars.col(0), coeff_rotation_regularization, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      SWITCH_DEFAULT;
    }
  }

  void NeedleProblemHelper::AddRotationContinuityCost(OptProb& prob, NeedleProblemInstancePtr pi) {
    switch (rotation_cost) {
      case UseRotationQuadraticCost: {
        prob.addCost(CostPtr(new RotationContinuityQuadraticCost(pi->phivars.col(0), coeff_rotation_continuity, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      case UseRotationL1Cost: {
        prob.addCost(CostPtr(new RotationContinuityL1Cost(pi->phivars.col(0), coeff_rotation_continuity_regularization, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      SWITCH_DEFAULT;
    }
  }

  void NeedleProblemHelper::AddSpeedCost(OptProb& prob, NeedleProblemInstancePtr pi) {
    prob.addCost(CostPtr(new ConstantSpeedCost(pi->Deltavar, coeff_speed, shared_from_this(), pi)));
    this->speed_costs.push_back(prob.getCosts().back());
  }

  void NeedleProblemHelper::ConfigureProblem(OptProb& prob) {
    for (int i = 0; i < n_needles; ++i) {
      NeedleProblemInstancePtr pi(new NeedleProblemInstance());
      pi->entry = entries[i];
      pi->final = finals[i];
      pi->init_traj = init_trajs[i];
      pi->init_control = init_controls[i];

      pi->T = Ts[i];
      pi->id = i;

      pi->helper = shared_from_this();
      pi->entry_position_error_relax = this->entry_position_error_relax[i];
      pi->entry_orientation_error_relax = this->entry_orientation_error_relax[i];
      pi->final_distance_error_relax = this->final_distance_error_relax[i];

      cout << "needle " << i << " relax" << endl;
      cout << pi->entry_position_error_relax.transpose() << endl;
      cout << pi->entry_orientation_error_relax << endl;
      cout << "needle " << i << " relax end" << endl;

      CreateVariables(prob, pi);
      InitLocalConfigurations(this->robots[i], prob, pi);
      InitTrajectory(prob, pi);
      AddRotationCost(prob, pi);
      AddCurvatureCost(prob, pi);

      //AddSpeedCost(prob, pi); //not important I guess

      AddEntryConstraint(prob, pi);

      if (this->channel_planning)
      {
        AddCurvatureConstraint(prob, pi);
        AddRotationConstraint(prob, pi);
        AddChannelConstraint(prob, pi);

        if (this->channel_continuity)
        {
          AddCurvatureContinuityCost(prob, pi);
          AddRotationContinuityCost(prob, pi);
        }
<commit_msg>final<commit_after>#include "needle_problem_helper.hpp"
#include "utils.hpp"
#include "costs.hpp"
#include "constraints.hpp"

namespace Needle {
  template<typename T, size_t N>
  T* end(T (&ra)[N]) {
    return ra + N;
  } 

  void NeedleProblemHelper::AddRotationCost(OptProb& prob, NeedleProblemInstancePtr pi) {
    switch (rotation_cost) {
      case UseRotationQuadraticCost: {
        prob.addCost(CostPtr(new RotationQuadraticCost(pi->phivars.col(0), coeff_rotation, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      case UseRotationL1Cost: {
        prob.addCost(CostPtr(new RotationL1Cost(pi->phivars.col(0), coeff_rotation_regularization, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      SWITCH_DEFAULT;
    }
  }

  void NeedleProblemHelper::AddRotationContinuityCost(OptProb& prob, NeedleProblemInstancePtr pi) {
    switch (rotation_cost) {
      case UseRotationQuadraticCost: {
        prob.addCost(CostPtr(new RotationContinuityQuadraticCost(pi->phivars.col(0), coeff_rotation_continuity, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      case UseRotationL1Cost: {
        prob.addCost(CostPtr(new RotationContinuityL1Cost(pi->phivars.col(0), coeff_rotation_continuity_regularization, shared_from_this())));
        this->rotation_costs.push_back(prob.getCosts().back());
        break;
      }
      SWITCH_DEFAULT;
    }
  }

  void NeedleProblemHelper::AddSpeedCost(OptProb& prob, NeedleProblemInstancePtr pi) {
    prob.addCost(CostPtr(new ConstantSpeedCost(pi->Deltavar, coeff_speed, shared_from_this(), pi)));
    this->speed_costs.push_back(prob.getCosts().back());
  }

  void NeedleProblemHelper::ConfigureProblem(OptProb& prob) {
    for (int i = 0; i < n_needles; ++i) {
      NeedleProblemInstancePtr pi(new NeedleProblemInstance());
      pi->entry = entries[i];
      pi->final = finals[i];
      pi->init_traj = init_trajs[i];
      pi->init_control = init_controls[i];

      pi->T = Ts[i];
      pi->id = i;

      pi->helper = shared_from_this();
      pi->entry_position_error_relax = this->entry_position_error_relax[i];
      pi->entry_orientation_error_relax = this->entry_orientation_error_relax[i];
      pi->final_distance_error_relax = this->final_distance_error_relax[i];

      cout << "needle " << i << " relax" << endl;
      cout << pi->entry_position_error_relax.transpose() << endl;
      cout << pi->entry_orientation_error_relax << endl;
      cout << "needle " << i << " relax end" << endl;

      CreateVariables(prob, pi);
      InitLocalConfigurations(this->robots[i], prob, pi);
      InitTrajectory(prob, pi);
      AddRotationCost(prob, pi);
      AddCurvatureCost(prob, pi);

      //AddSpeedCost(prob, pi); //not important I guess

      AddEntryConstraint(prob, pi);

      if (this->channel_planning)
      {
        AddCurvatureConstraint(prob, pi);
        AddRotationConstraint(prob, pi);
        AddChannelConstraint(prob, pi);

        if (this->channel_continuity)
        {
          //AddCurvatureContinuityCost(prob, pi);
          AddRotationContinuityCost(prob, pi);
        }

{'fraction_non_alphanumeric': 0.1303412234860129, 'fraction_numerical': 0.0027666769136181985, 'mean_word_length': 3.5599159074982483, 'pattern_counts': {'":': 0, '<': 55, '<?xml version=': 0, '>': 61, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, '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': '1780591', 'n_tokens_mistral': 2174, 'n_tokens_neox': 2095, 'n_words': 324}