<commit_before>/*******************************************************************************
 * Copyright (c) 2013 <NAME> <<EMAIL>> and contributors.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *******************************************************************************/
package optimisation.problem.robotic.mechanism;

import optimisation.util.Algebra;
import optimisation.util.Geometry;
import optimisation.util.Sparse;
import arma.Arma;
import arma.Mat;
import arma.Op;

/**
 * A {@code n}(P̲)UP̲S robot, with {@code n} as the number of kinematic chains.
 * 
 * @author <NAME> <<EMAIL>>
 * 
 * @see "<NAME> al.: Influence of Kinematic Redundancy on the Singularity-free Workspace of Parallel Kinematic Machines, Frontiers of Mechanical Engineering, Vol. 7(2), pp. 120-134, 2012"
 */
public class Robot_NPUPS extends Robot {

  /**
   * The XY-position of the end effector joint for each kinematic chain per row.
   */
  protected Mat _endEffectorJointXYZPosition;

  /**
   * The XY-position of the start of the prismatic joint for each kinematic chain per row.
   */
  protected Mat _prismaticJointStartXYZPosition;

  /**
   * The XY-distance between the start and the end of the prismatic joint for each kinematic chain per row.
   */
  protected Mat _prismaticJointXYZDistance;

  // Pre-allocated memory

  /**
   * The Jacobian
   */
  protected Mat _jacobian;

  /**
   * The direct Jacobian
   */
  protected Mat _directJacobian;

  /**
   * The inverse Jacobian
   */
  protected Mat _inverseJacobian;

  /**
   * Used to specify the parameter for an actual {@code n}(P̲)R̲RR robot.
   * <p>
   * To denote that an optional prismatic joint is not present in the actual robot configuration for a kinematic chain,
   * set the XY-position of the start of the prismatic joint equal to its end for its corresponding column.
   * <p>
   * <b>Important note:</b> Some parameter must be relative to the origin of the base, while others must be relative to
   * the end effector.
   * 
   * @param endEffectorJointXYZPosition The XYZ-position of the end effector joint for each kinematic chain per row.
   *          <b>Note:</b> Must be relative to the origin of the end effector.
   * @param prismaticJointStartXYZPosition The XYZ-position of the start of the prismatic joint for each kinematic chain
   *          per row. <b>Note:</b> Must be relative to the origin of the base.
   * @param prismaticJointEndXYZPosition The XYZ-positions of the end of the prismatic joint for each kinematic chain
   *          per
   *          row. <b>Note:</b> Must be relative to the origin of the base.
   */
  protected Robot_NPUPS(Mat endEffectorJointXYZPosition,
                        Mat prismaticJointStartXYZPosition,
                        Mat prismaticJointEndXYZPosition) {
    _endEffectorJointXYZPosition = endEffectorJointXYZPosition;
    _prismaticJointStartXYZPosition = prismaticJointStartXYZPosition;

    _prismaticJointXYZDistance = _prismaticJointStartXYZPosition.minus(prismaticJointStartXYZPosition);

    // Pre-allocated memory
    int numberOfKinematicChains = _endEffectorJointXYZPosition.n_rows;

    _jacobian = new Mat(6, numberOfKinematicChains + Sparse.nnz(_prismaticJointXYZDistance));
    _directJacobian = new Mat(numberOfKinematicChains, 6);
    _inverseJacobian = new Mat(numberOfKinematicChains, numberOfKinematicChains + Sparse.nnz(_prismaticJointXYZDistance));
  }

  @Override
  public Mat getJacobian(Mat prismaticJointRelativeDistances, Mat XYZposition, double rollAngle, double pitchAngle, double yawAngle) {
    // Rotate the end effector joint positions if the end effector was rotated.
    Mat rotatedEndEffectorJointXYZPosition = new Mat(_endEffectorJointXYZPosition);
    if (rollAngle != 0 || pitchAngle != 0 || yawAngle != 0) {
      Mat rotationMatrix = Geometry.getZYXTaitBryanAnglesRotationMatrix(rollAngle, pitchAngle, yawAngle);

      for (int i = 0; i < rotatedEndEffectorJointXYZPosition.n_rows; i++) {
        rotatedEndEffectorJointXYZPosition.row(i, Op.EQUAL, rotatedEndEffectorJointXYZPosition.row(i).times(rotationMatrix));
      }
    }

    // The XY-position of the end effector joint for each kinematic chain per row, relative to the origin of the base.
    // A copy is made since the XY-Position relative to the orign of the end effector is later needed to calculate the<commit_msg>Fixed Robot_NPUPS modell<commit_after>/*******************************************************************************
 * Copyright (c) 2013 <NAME> <<EMAIL>> and contributors.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *******************************************************************************/
package optimisation.problem.robotic.mechanism;

import optimisation.util.Algebra;
import optimisation.util.Geometry;
import arma.Arma;
import arma.Mat;
import arma.Op;

/**
 * A {@code n}(P̲)UP̲S robot, with {@code n} as the number of kinematic chains.
 * 
 * @author <NAME> <<EMAIL>>
 * 
 * @see "<NAME> al.: Influence of Kinematic Redundancy on the Singularity-free Workspace of Parallel Kinematic Machines, Frontiers of Mechanical Engineering, Vol. 7(2), pp. 120-134, 2012"
 */
public class Robot_NPUPS extends Robot {

  /**
   * The XY-position of the end effector joint for each kinematic chain per row.
   */
  protected Mat _endEffectorJointXYZPosition;

  /**
   * The XY-position of the start of the prismatic joint for each kinematic chain per row.
   */
  protected Mat _prismaticJointStartXYZPosition;

  /**
   * The XY-distance between the start and the end of the prismatic joint for each kinematic chain per row.
   */
  protected Mat _prismaticJointXYZDistance;
  
  /**
   * The indices of kinematic chains with redundant prismatic joints
   */
  protected Mat _kinematicChainsWithRedundancy;

  // Pre-allocated memory

  /**
   * The Jacobian
   */
  protected Mat _jacobian;

  /**
   * The direct Jacobian
   */
  protected Mat _directJacobian;

  /**
   * The inverse Jacobian
   */
  protected Mat _inverseJacobian;

  /**
   * Used to specify the parameter for an actual {@code n}(P̲)R̲RR robot.
   * <p>
   * To denote that an optional prismatic joint is not present in the actual robot configuration for a kinematic chain,
   * set the XY-position of the start of the prismatic joint equal to its end for its corresponding column.
   * <p>
   * <b>Important note:</b> Some parameter must be relative to the origin of the base, while others must be relative to
   * the end effector.
   * 
   * @param endEffectorJointXYZPosition The XYZ-position of the end effector joint for each kinematic chain per row.
   *          <b>Note:</b> Must be relative to the origin of the end effector.
   * @param prismaticJointStartXYZPosition The XYZ-position of the start of the prismatic joint for each kinematic chain
   *          per row. <b>Note:</b> Must be relative to the origin of the base.
   * @param prismaticJointEndXYZPosition The XYZ-positions of the end of the prismatic joint for each kinematic chain
   *          per
   *          row. <b>Note:</b> Must be relative to the origin of the base.
   */
  protected Robot_NPUPS(Mat endEffectorJointXYZPosition,
                        Mat prismaticJointStartXYZPosition,
                        Mat prismaticJointEndXYZPosition) {
    _endEffectorJointXYZPosition = endEffectorJointXYZPosition;
    _prismaticJointStartXYZPosition = prismaticJointStartXYZPosition;

    _prismaticJointXYZDistance = _prismaticJointStartXYZPosition.minus(prismaticJointStartXYZPosition);

    // Pre-allocated memory
    _kinematicChainsWithRedundancy = Arma.find(Algebra.normMat(_prismaticJointXYZDistance, 2, 1));
    int numberOfKinematicChains = _endEffectorJointXYZPosition.n_rows;
    int numberOfKinematicChainsWithRedundancy = _kinematicChainsWithRedundancy.n_elem;

    _jacobian = new Mat(6, numberOfKinematicChains + numberOfKinematicChainsWithRedundancy);
    _directJacobian = new Mat(numberOfKinematicChains, 6);
    _inverseJacobian = new Mat(numberOfKinematicChains, numberOfKinematicChains + numberOfKinematicChainsWithRedundancy);
  }

  @Override
  public Mat getJacobian(Mat prismaticJointRelativeDistances, Mat XYZposition, double rollAngle, double pitchAngle, double yawAngle) {
    // Rotate the end effector joint positions if the end effector was rotated.
    Mat rotatedEndEffectorJointXYZPosition = new Mat(_endEffectorJointXYZPosition);
    if (rollAngle != 0 || pitchAngle != 0 || yawAngle != 0) {
      Mat rotationMatrix = Geometry.getZYXTaitBryanAnglesRotationMatrix(rollAngle, pitchAngle, yawAngle);

      for (int i = 0; i < rotatedEndEffectorJointXYZPosition.n_rows; i++) {
        rotatedEndEffectorJointXYZPosition.row(i, Op.EQUAL, rotatedEndEffectorJointXYZPosition.row(i).times(rotationMatrix));
      }
    }

    // The XY-position of the end effector joint for each kinematic chain per row, relative to the origin of the base.
    // A copy is made since the XY-Position relative to the orign of the end effector is later needed to calculate the
{'fraction_non_alphanumeric': 0.10683669095585084, 'fraction_numerical': 0.004929275610801543, 'mean_word_length': 3.8584070796460175, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 37, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2403611', 'n_tokens_mistral': 2937, 'n_tokens_neox': 2585, 'n_words': 910}