<commit_before>/**
 * @file kinematics.h
 * @brief This defines kinematic related utilities.
 *
 * @author <NAME>
 * @date June 3, 2016
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2016, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_

#include <ros/console.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <XmlRpcException.h>
#include <moveit_msgs/PositionConstraint.h>
#include <moveit_msgs/OrientationConstraint.h>


namespace stomp_moveit
{
namespace utils
{

/**
 * @namespace stomp_moveit::utils::kinematics
 * @brief Utility functions related to finding Inverse Kinematics solutions
 */
namespace kinematics
{

  const static double EPSILON = 0.011;  /**< @brief Used in dampening the matrix pseudo inverse calculation */
  const static double LAMBDA = 0.01;    /**< @brief Used in dampening the matrix pseudo inverse calculation */

  /**
   * @struct stomp_moveit::utils::KinematicConfig
   * @brief Convenience structure that contains the variables used in solving for an ik solution.
   */
  struct KinematicConfig
  {
    /** @name Constraint Parameters
     * The constraints on the cartesian goal
     */
    Eigen::Array<int,6,1> constrained_dofs = Eigen::Array<int,6,1>::Ones(); /**< @brief  A vector of the form [x y z rx ry rz] filled with 0's and 1's
                                                                                        to indicate an unconstrained or fully constrained DOF. **/
    Eigen::Affine3d tool_goal_pose = Eigen::Affine3d::Identity();           /**< @brief  The desired tool pose. **/

    /** @name Support Parameters
     * Used at each iteration until a solution is found
     */
    Eigen::ArrayXd joint_update_rates = Eigen::ArrayXd::Zero(1,1);          /**< @brief The weights to be applied to each update during every iteration [num_dimensions x 1]. **/
    Eigen::Array<double,6,1> cartesian_convergence_thresholds = Eigen::Array<double,6,1>::Zero(); /**< @brief  The error margin for each dimension of the twist vector [6 x 1]. **/
    Eigen::VectorXd init_joint_pose = Eigen::ArrayXd::Zero(1,1);            /**< @brief  Seed joint pose [num_dimension x 1]. **/
    int max_iterations = 100;                                               /**< @brief  The maximum number of iterations that the algorithm will run until convergence is reached. **/

    /** @name Null Space Parameters
     * Used in exploding the task manifold null space
     */
    Eigen::ArrayXd null_proj_weights = Eigen::ArrayXd::Zero(1,1);  /**< @brief Weights to be applied to the null space vector */
    Eigen::VectorXd null_space_vector = Eigen::VectorXd::Zero(1);  /**< @brief Null space vector that is used to exploit the Jacobian's null space */

  };

  /**
   * @brief Populates a Kinematic Config struct from the position and orientation constraints requested;
   * @param group             A pointer to the JointModelGroup
   * @param pc                The position constraint message
   * @param oc                The orientation constraint message.  Absolute tolerances greater than 2PI will be considered unconstrained.
   * @param init_joint_pose   The initial joint values
   * @param kc                The output KinematicConfig object
   * @return  True if succeeded, false otherwise
   */
  static bool createKinematicConfig(const moveit::core::JointModelGroup* group,
                                               const moveit_msgs::PositionConstraint& pc,const moveit_msgs::OrientationConstraint& oc,
                                               const Eigen::VectorXd& init_joint_pose,KinematicConfig& kc)
  {
    int num_joints = group->getActiveJointModelNames().size();
    if(num_joints != init_joint_pose.size())
    {
      ROS_ERROR("Initial joint pose has an incorrect number of joints");
      return false;
    }

    // tool pose position
    kc.tool_goal_pose.setIdentity();
    auto& v = pc.constraint_region.primitive_poses[0].position;
    kc.tool_goal_pose.translation() = Eigen::Vector3d(v.x,v.y,v.z);

    // tool pose orientation
    Eigen::Quaterniond q;
    tf::quaternionMsgToEigen(oc.orientation,q);
    kc.tool_goal_pose.rotate(q);

    // defining constraints
    kc.constrained_dofs << 1, 1, 1, 1, 1, 1;

    // position tolerance
    const shape_msgs::SolidPrimitive& bv = pc.constraint_region.primitives[0];
    if(bv.type != shape_msgs::SolidPrimitive::BOX || bv.dimensions.size() != 3)
    {
      ROS_ERROR("Position constraint incorrectly defined, a BOX region is expected");
      return false;
    }

    using SP = shape_msgs::SolidPrimitive;
    kc.cartesian_convergence_thresholds[0] = bv.dimensions[SP::BOX_X];
    kc.cartesian_convergence_thresholds[1] = bv.dimensions[SP::BOX_Y];
    kc.cartesian_convergence_thresholds[2] = bv.dimensions[SP::BOX_Z];

    // orientation tolerance
    kc.cartesian_convergence_thresholds[3] = oc.absolute_x_axis_tolerance;
    kc.cartesian_convergence_thresholds[4] = oc.absolute_y_axis_tolerance;
    kc.cartesian_convergence_thresholds[5] = oc.absolute_z_axis_tolerance;

    // unconstraining orientation dofs that exceed 2pi
    for(std::size_t i = 3; i < kc.cartesian_convergence_thresholds.size(); i++)
    {
      auto v = kc.cartesian_convergence_thresholds[i];
      kc.constrained_dofs[i] = v >= 2*M_PI ? 0 : 1;
    }

    // additional variables
    kc.joint_update_rates = Eigen::ArrayXd::Constant(num_joints,0.5f);
    kc.init_joint_pose = init_joint_pose;
    kc.max_iterations = 100;

    return true;
  }

  /**
   * @brief Populates a Kinematic Config struct from the position and orientation constraints requested;
   * @param group         A pointer to the JointModelGroup
   * @param pc            The position constraint message
   * @param oc            The orientation constraint message.  Absolute tolerances greater than 2PI will be considered unconstrained.
   * @param start_state   The start robot state message<commit_msg>Add default values if primitive is not given & take position info from "target_point_offset" field<commit_after>/**
 * @file kinematics.h
 * @brief This defines kinematic related utilities.
 *
 * @author <NAME>
 * @date June 3, 2016
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2016, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_

#include <ros/console.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <XmlRpcException.h>
#include <moveit_msgs/PositionConstraint.h>
#include <moveit_msgs/OrientationConstraint.h>


namespace stomp_moveit
{
namespace utils
{

/**
 * @namespace stomp_moveit::utils::kinematics
 * @brief Utility functions related to finding Inverse Kinematics solutions
 */
namespace kinematics
{

  const static double EPSILON = 0.011;  /**< @brief Used in dampening the matrix pseudo inverse calculation */
  const static double LAMBDA = 0.01;    /**< @brief Used in dampening the matrix pseudo inverse calculation */
  const static double default_position_tolerance_ = 0.1; /**< @brief Used in setting IK tolerance */

  /**
   * @struct stomp_moveit::utils::KinematicConfig
   * @brief Convenience structure that contains the variables used in solving for an ik solution.
   */
  struct KinematicConfig
  {
    /** @name Constraint Parameters
     * The constraints on the cartesian goal
     */
    Eigen::Array<int,6,1> constrained_dofs = Eigen::Array<int,6,1>::Ones(); /**< @brief  A vector of the form [x y z rx ry rz] filled with 0's and 1's
                                                                                        to indicate an unconstrained or fully constrained DOF. **/
    Eigen::Affine3d tool_goal_pose = Eigen::Affine3d::Identity();           /**< @brief  The desired tool pose. **/

    /** @name Support Parameters
     * Used at each iteration until a solution is found
     */
    Eigen::ArrayXd joint_update_rates = Eigen::ArrayXd::Zero(1,1);          /**< @brief The weights to be applied to each update during every iteration [num_dimensions x 1]. **/
    Eigen::Array<double,6,1> cartesian_convergence_thresholds = Eigen::Array<double,6,1>::Zero(); /**< @brief  The error margin for each dimension of the twist vector [6 x 1]. **/
    Eigen::VectorXd init_joint_pose = Eigen::ArrayXd::Zero(1,1);            /**< @brief  Seed joint pose [num_dimension x 1]. **/
    int max_iterations = 100;                                               /**< @brief  The maximum number of iterations that the algorithm will run until convergence is reached. **/

    /** @name Null Space Parameters
     * Used in exploding the task manifold null space
     */
    Eigen::ArrayXd null_proj_weights = Eigen::ArrayXd::Zero(1,1);  /**< @brief Weights to be applied to the null space vector */
    Eigen::VectorXd null_space_vector = Eigen::VectorXd::Zero(1);  /**< @brief Null space vector that is used to exploit the Jacobian's null space */

  };

  /**
   * @brief Populates a Kinematic Config struct from the position and orientation constraints requested;
   * @param group             A pointer to the JointModelGroup
   * @param pc                The position constraint message
   * @param oc                The orientation constraint message.  Absolute tolerances greater than 2PI will be considered unconstrained.
   * @param init_joint_pose   The initial joint values
   * @param kc                The output KinematicConfig object
   * @return  True if succeeded, false otherwise
   */
  static bool createKinematicConfig(const moveit::core::JointModelGroup* group,
                                               const moveit_msgs::PositionConstraint& pc,const moveit_msgs::OrientationConstraint& oc,
                                               const Eigen::VectorXd& init_joint_pose,KinematicConfig& kc)
  {
    int num_joints = group->getActiveJointModelNames().size();
    if(num_joints != init_joint_pose.size())
    {
      ROS_ERROR("Initial joint pose has an incorrect number of joints");
      return false;
    }

    // POSITION constraints

    // tool pose position
    kc.tool_goal_pose.setIdentity();

    // we may take position constraints from the "BoundingVolume  constraint_region" of the MotionPlanRequest message
    if(pc.constraint_region.primitive_poses.size() > 0)
    {
      ROS_INFO("createKinematicConfig: Using constraint_region from message to determine tolerances");
      const auto& v = pc.constraint_region.primitive_poses[0].position;
      kc.tool_goal_pose.translation() = Eigen::Vector3d(v.x,v.y,v.z);

      // position tolerance
      const shape_msgs::SolidPrimitive& bv = pc.constraint_region.primitives[0];
      if(bv.type != shape_msgs::SolidPrimitive::BOX || bv.dimensions.size() != 3)
      {
        ROS_ERROR("Position constraint incorrectly defined, a BOX region is expected");
        return false;
      }

      using SP = shape_msgs::SolidPrimitive;
      kc.cartesian_convergence_thresholds[0] = bv.dimensions[SP::BOX_X];
      kc.cartesian_convergence_thresholds[1] = bv.dimensions[SP::BOX_Y];
      kc.cartesian_convergence_thresholds[2] = bv.dimensions[SP::BOX_Z];
    }
    // or we use "target_point_offset" to define the 3DOF position
    else
    {
      const auto& v = pc.target_point_offset;
      kc.tool_goal_pose.translation() = Eigen::Vector3d(v.x,v.y,v.z);

      ROS_INFO_STREAM("constraint_region not available in message, using default position tolerance of " << default_position_tolerance_);
      kc.cartesian_convergence_thresholds[0] = default_position_tolerance_;
      kc.cartesian_convergence_thresholds[1] = default_position_tolerance_;
      kc.cartesian_convergence_thresholds[2] = default_position_tolerance_;
    }
    // ORIENTATION constraints

    // tool pose orientation
    Eigen::Quaterniond q;
    tf::quaternionMsgToEigen(oc.orientation,q);
    kc.tool_goal_pose.rotate(q);

    // orientation tolerance
    kc.cartesian_convergence_thresholds[3] = oc.absolute_x_axis_tolerance;
    kc.cartesian_convergence_thresholds[4] = oc.absolute_y_axis_tolerance;
    kc.cartesian_convergence_thresholds[5] = oc.absolute_z_axis_tolerance;

    // unconstraining orientation dofs that exceed 2pi
    for(std::size_t i = 3; i < kc.cartesian_convergence_thresholds.size(); i++)
    {
      auto v = kc.cartesian_convergence_thresholds[i];
      kc.constrained_dofs[i] = v >= 2*M_PI ? 0 : 1;
    }

    // additional variables
    kc.joint_update_rates = Eigen::ArrayXd::Constant(num_joints,0.5f);
    kc.init_joint_pose = init_joint_pose;
    kc.max_iterations = 100;
    kc.constrained_dofs << 1, 1, 1, 1, 1, 1;

    return true;
  }

  /**
   * @brief Populates a Kinematic Config struct from the position and orientation constraints requested;
   * @param group         A pointer to the JointModelGroup
   * @param pc            The position constraint message
   * @param oc            The orientation constraint message.  Absolute tolerances greater than 2PI will be considered unconstrained.
   * @param start_state   The start robot state message
{'fraction_non_alphanumeric': 0.09415188845268715, 'fraction_numerical': 0.010559090293759307, 'mean_word_length': 3.32396839332748, 'pattern_counts': {'":': 0, '<': 60, '<?xml version=': 0, '>': 36, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 20, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3134124', 'n_tokens_mistral': 4581, 'n_tokens_neox': 4231, 'n_words': 1264}