<commit_before>#include <hop_bits/optimisationProblem/robotic/model/parallelKinematicMachine_6pups.hpp>

// HOP
#include <hop_bits/helper/geometry.hpp>

namespace hop {
  namespace robotic {
    ParallelKinematicMachine_6PUPS::ParallelKinematicMachine_6PUPS() noexcept
      : ParallelKinematicMachine_6PUPS(
          arma::Mat<double>::fixed<3, 6>({
            -0.025561381023353, 0.086293776138137, 0.12,
            0.025561381023353, 0.086293776138137, 0.12,
            0.087513292835791, -0.021010082747031, 0.12,
            0.061951911812438, -0.065283693391106, 0.12,
            -0.061951911812438, -0.065283693391106, 0.12,
            -0.087513292835791, -0.021010082747032, 0.12}),
          arma::Mat<double>::fixed<3, 6>({
            -0.463708870031622, 0.417029254828353, -0.346410161513775,
            0.463708870031622, 0.417029254828353, -0.346410161513775,
            0.593012363818459, 0.193069033993384, -0.346410161513775,
            0.129303493786838, -0.610098288821738, -0.346410161513775,
            -0.129303493786837, -0.610098288821738, -0.346410161513775,
            -0.593012363818459, 0.193069033993384, -0.346410161513775}),
          arma::Mat<double>::fixed<3, 6>({
            -0.247202519085512, 0.292029254828353, 0.086602540378444,
            0.247202519085512, 0.292029254828353, 0.086602540378444,
            0.376506012872349, 0.068069033993384, 0.086602540378444,
            0.129303493786838, -0.360098288821738, 0.086602540378444,
            -0.129303493786837, -0.360098288821738, 0.086602540378444,
            -0.376506012872349, 0.068069033993384, 0.086602540378444}),
          arma::Row<double>::fixed<6>({
            0.39, 0.39, 0.39, 0.39, 0.39, 0.39}),
          arma::Row<double>::fixed<6>({
            0.95, 0.95, 0.95, 0.95, 0.95, 0.95})) {

    }

    ParallelKinematicMachine_6PUPS::ParallelKinematicMachine_6PUPS(
        const arma::Mat<double>::fixed<3, 6>& relativeEndEffectorJoints,
        const arma::Mat<double>::fixed<3, 6>& redundantJointStarts,
        const arma::Mat<double>::fixed<3, 6>& redundantJointEnds,
        const arma::Row<double>::fixed<6>& minimalActiveJointActuations,
        const arma::Row<double>::fixed<6>& maximalActiveJointActuations) noexcept
      : endEffectorJointsRelative_(relativeEndEffectorJoints),
        redundantJointStarts_(redundantJointStarts),
        redundantJointEnds_(redundantJointEnds),
        minimalActiveJointActuations_(minimalActiveJointActuations),
        maximalActiveJointActuations_(maximalActiveJointActuations),
        redundantJointsStartToEnd_(redundantJointEnds_ - redundantJointStarts_),
        redundantJointIndicies_(arma::find(arma::any(redundantJointsStartToEnd_))),
        redundantJointAngles_(3, redundantJointIndicies_.n_elem) {
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; ++n) {
        const double& redundantJointXAngle = std::atan2(redundantJointsStartToEnd_.at(1, n), redundantJointsStartToEnd_.at(0, n));
        const double& redundantJointYAngle = std::atan2(redundantJointsStartToEnd_.at(2, n), redundantJointsStartToEnd_.at(1, n));
        redundantJointAngles_.col(n) = arma::Col<double>::fixed<3>({std::cos(redundantJointXAngle) * std::cos(redundantJointYAngle), std::sin(redundantJointXAngle) * std::cos(redundantJointYAngle), std::sin(redundantJointYAngle)});
      }
    }

    std::vector<arma::Mat<double>> ParallelKinematicMachine_6PUPS::getModelCharacterisation(
        const arma::Col<double>& endEffectorPose,
        const arma::Mat<double>& redundantJointActuations) const noexcept {
      std::vector<arma::Mat<double>> modelCharacterisation;

      if (arma::any(arma::vectorise(redundantJointActuations < 0)) || arma::any(arma::vectorise(redundantJointActuations > 1))) {
        throw std::runtime_error("All values for the actuation of redundantion joints must be between [0, 1].");
      }

      const arma::Col<double>::fixed<3>& endEffector = endEffectorPose.subvec(0, 2);
      const double& endEffectorRollAngle = endEffectorPose.at(3);
      const double& endEffectorPitchAngle = endEffectorPose.at(4);
      const double& endEffectorYawAngle = endEffectorPose.at(5);

      arma::Mat<double>::fixed<3, 6> baseJoints = redundantJointStarts_;
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; n++) {
        const arma::uword& redundantJointIndex = redundantJointIndicies_.at(n);
        baseJoints.col(redundantJointIndex) += redundantJointActuations.at(redundantJointIndex) * redundantJointsStartToEnd_.col(redundantJointIndex);
      }

      const arma::Mat<double>::fixed<3, 6>& endEffectorJointsRotated = get3DRotationMatrix(endEffectorRollAngle, endEffectorPitchAngle, endEffectorYawAngle) * endEffectorJointsRelative_;
      arma::Mat<double>::fixed<3, 6> endEffectorJoints = endEffectorJointsRotated;
      endEffectorJoints.each_col() += endEffector;

      modelCharacterisation.push_back(baseJoints);
      modelCharacterisation.push_back(endEffectorJoints);

      return modelCharacterisation;<commit_msg>devel: Removed unused temporary copy<commit_after>#include <hop_bits/optimisationProblem/robotic/model/parallelKinematicMachine_6pups.hpp>

// HOP
#include <hop_bits/helper/geometry.hpp>

namespace hop {
  namespace robotic {
    ParallelKinematicMachine_6PUPS::ParallelKinematicMachine_6PUPS() noexcept
      : ParallelKinematicMachine_6PUPS(
          arma::Mat<double>::fixed<3, 6>({
            -0.025561381023353, 0.086293776138137, 0.12,
            0.025561381023353, 0.086293776138137, 0.12,
            0.087513292835791, -0.021010082747031, 0.12,
            0.061951911812438, -0.065283693391106, 0.12,
            -0.061951911812438, -0.065283693391106, 0.12,
            -0.087513292835791, -0.021010082747032, 0.12}),
          arma::Mat<double>::fixed<3, 6>({
            -0.463708870031622, 0.417029254828353, -0.346410161513775,
            0.463708870031622, 0.417029254828353, -0.346410161513775,
            0.593012363818459, 0.193069033993384, -0.346410161513775,
            0.129303493786838, -0.610098288821738, -0.346410161513775,
            -0.129303493786837, -0.610098288821738, -0.346410161513775,
            -0.593012363818459, 0.193069033993384, -0.346410161513775}),
          arma::Mat<double>::fixed<3, 6>({
            -0.247202519085512, 0.292029254828353, 0.086602540378444,
            0.247202519085512, 0.292029254828353, 0.086602540378444,
            0.376506012872349, 0.068069033993384, 0.086602540378444,
            0.129303493786838, -0.360098288821738, 0.086602540378444,
            -0.129303493786837, -0.360098288821738, 0.086602540378444,
            -0.376506012872349, 0.068069033993384, 0.086602540378444}),
          arma::Row<double>::fixed<6>({
            0.39, 0.39, 0.39, 0.39, 0.39, 0.39}),
          arma::Row<double>::fixed<6>({
            0.95, 0.95, 0.95, 0.95, 0.95, 0.95})) {

    }

    ParallelKinematicMachine_6PUPS::ParallelKinematicMachine_6PUPS(
        const arma::Mat<double>::fixed<3, 6>& relativeEndEffectorJoints,
        const arma::Mat<double>::fixed<3, 6>& redundantJointStarts,
        const arma::Mat<double>::fixed<3, 6>& redundantJointEnds,
        const arma::Row<double>::fixed<6>& minimalActiveJointActuations,
        const arma::Row<double>::fixed<6>& maximalActiveJointActuations) noexcept
      : endEffectorJointsRelative_(relativeEndEffectorJoints),
        redundantJointStarts_(redundantJointStarts),
        redundantJointEnds_(redundantJointEnds),
        minimalActiveJointActuations_(minimalActiveJointActuations),
        maximalActiveJointActuations_(maximalActiveJointActuations),
        redundantJointsStartToEnd_(redundantJointEnds_ - redundantJointStarts_),
        redundantJointIndicies_(arma::find(arma::any(redundantJointsStartToEnd_))),
        redundantJointAngles_(3, redundantJointIndicies_.n_elem) {
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; ++n) {
        const double& redundantJointXAngle = std::atan2(redundantJointsStartToEnd_.at(1, n), redundantJointsStartToEnd_.at(0, n));
        const double& redundantJointYAngle = std::atan2(redundantJointsStartToEnd_.at(2, n), redundantJointsStartToEnd_.at(1, n));
        redundantJointAngles_.col(n) = arma::Col<double>::fixed<3>({std::cos(redundantJointXAngle) * std::cos(redundantJointYAngle), std::sin(redundantJointXAngle) * std::cos(redundantJointYAngle), std::sin(redundantJointYAngle)});
      }
    }

    std::vector<arma::Mat<double>> ParallelKinematicMachine_6PUPS::getModelCharacterisation(
        const arma::Col<double>& endEffectorPose,
        const arma::Mat<double>& redundantJointActuations) const noexcept {
      std::vector<arma::Mat<double>> modelCharacterisation;

      if (arma::any(arma::vectorise(redundantJointActuations < 0)) || arma::any(arma::vectorise(redundantJointActuations > 1))) {
        throw std::runtime_error("All values for the actuation of redundantion joints must be between [0, 1].");
      }

      const arma::Col<double>::fixed<3>& endEffector = endEffectorPose.subvec(0, 2);
      const double& endEffectorRollAngle = endEffectorPose.at(3);
      const double& endEffectorPitchAngle = endEffectorPose.at(4);
      const double& endEffectorYawAngle = endEffectorPose.at(5);

      arma::Mat<double>::fixed<3, 6> baseJoints = redundantJointStarts_;
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; n++) {
        const arma::uword& redundantJointIndex = redundantJointIndicies_.at(n);
        baseJoints.col(redundantJointIndex) += redundantJointActuations.at(redundantJointIndex) * redundantJointsStartToEnd_.col(redundantJointIndex);
      }

      arma::Mat<double>::fixed<3, 6> endEffectorJoints = get3DRotationMatrix(endEffectorRollAngle, endEffectorPitchAngle, endEffectorYawAngle) * endEffectorJointsRelative_;
      endEffectorJoints.each_col() += endEffector;

      modelCharacterisation.push_back(baseJoints);
      modelCharacterisation.push_back(endEffectorJoints);

      return modelCharacterisation;
{'fraction_non_alphanumeric': 0.12160814046288906, 'fraction_numerical': 0.1737829209896249, 'mean_word_length': 4.081094779523569, 'pattern_counts': {'":': 0, '<': 83, '<?xml version=': 0, '>': 79, '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': '5327178', 'n_tokens_mistral': 4751, 'n_tokens_neox': 3481, 'n_words': 452}