<commit_before>/*********************************************************************
*
*  Copyright (c) 2009, <NAME>, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

// Author(s): <NAME>

#include <grasp_place_evaluation/grasp_evaluator_fast.h>

using moveit_manipulation_msgs::GraspResult;
using moveit_msgs::MoveItErrorCodes;

namespace grasp_place_evaluation {

GraspEvaluatorFast::GraspEvaluatorFast(const planning_models::KinematicModelConstPtr& kmodel,
                                       const std::map<std::string, kinematics::KinematicsBasePtr>& solver_map)
  : GraspEvaluator(), 
    InterpolationEvaluator(kmodel, solver_map)
{  
}

void GraspEvaluatorFast::testGrasps(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                    const planning_models::KinematicState* seed_state,
                                    const moveit_manipulation_msgs::PickupGoal& pickup_goal,
                                    const geometry_msgs::Vector3& approach_direction,
                                    const std::vector<moveit_manipulation_msgs::Grasp>& grasps,
                                    GraspExecutionInfoVector &execution_info,
                                    bool return_on_first_hit) 
  
{
  ros::WallTime start = ros::WallTime::now();

  std::map<unsigned int, unsigned int> outcome_count;
    
  planning_models::KinematicState state(*seed_state);

  std::map<std::string, double> planning_scene_state_values;
  state.getStateValues(planning_scene_state_values);

  std::string end_effector_group = planning_scene->getKinematicModel()->getJointModelGroup(pickup_goal.arm_name)->getAttachedEndEffectorGroupName();
  // links are ordered by the order seen by depth-first. This means that for a chain the last link is the tip of the chain
  std::string tip_link = planning_scene->getKinematicModel()->getJointModelGroup(pickup_goal.arm_name)->getLinkModelNames().back();
  
  std::vector<std::string> end_effector_links = planning_scene->getKinematicModel()->getJointModelGroup(end_effector_group)->getLinkModelNames();
  std::vector<std::string> arm_links = planning_scene->getKinematicModel()->getJointModelGroup(pickup_goal.arm_name)->getLinkModelNames();
  
  collision_detection::AllowedCollisionMatrix original_acm = planning_scene->getAllowedCollisionMatrix();
  collision_detection::AllowedCollisionMatrix group_disable_acm = original_acm;//planning_scene->disableCollisionsForNonUpdatedLinks(pickup_goal.arm_name);

  collision_detection::AllowedCollisionMatrix object_disable_acm = group_disable_acm;
  object_disable_acm.setEntry(pickup_goal.collision_object_name, end_effector_links, true); 
  collision_detection::AllowedCollisionMatrix object_support_disable_acm = object_disable_acm;
  if(!pickup_goal.collision_support_surface_name.empty()) {
    //will come into play when object is attached
    object_support_disable_acm.setEntry(pickup_goal.collision_support_surface_name, pickup_goal.collision_object_name, true); 
    if(pickup_goal.allow_gripper_support_collision)
    {
      if(pickup_goal.collision_support_surface_name == "\"all\"")
      {
        for(unsigned int i = 0; i < end_effector_links.size(); i++){
          object_support_disable_acm.setDefaultEntry(end_effector_links[i], true);
        }
      } else {
        object_support_disable_acm.setEntry(pickup_goal.collision_support_surface_name, end_effector_links, true); 
      }
    }
  }
  collision_detection::AllowedCollisionMatrix object_all_arm_disable_acm = object_disable_acm;
  collision_detection::AllowedCollisionMatrix object_support_all_arm_disable_acm = object_support_disable_acm;
  collision_detection::AllowedCollisionMatrix group_all_arm_disable_acm = group_disable_acm;

  //turning off collisions for the arm associated with this end effector
  for(unsigned int i = 0; i < arm_links.size(); i++) {
    object_all_arm_disable_acm.setDefaultEntry(arm_links[i], true);
    object_support_all_arm_disable_acm.setDefaultEntry(arm_links[i], true);
    group_all_arm_disable_acm.setDefaultEntry(arm_links[i], true);
  }
  
  //first we apply link padding for grasp check
  //cm->applyLinkPaddingToCollisionSpace(linkPaddingForGrasp(pickup_goal));
    
  //setup that's not grasp specific  
  std_msgs::Header target_header;
  target_header.frame_id = pickup_goal.target.reference_frame_id;

  bool in_object_frame = false;
  Eigen::Affine3d obj_pose(Eigen::Affine3d::Identity());

  if(pickup_goal.target.reference_frame_id == pickup_goal.collision_object_name) {
    moveit_msgs::CollisionObject co;
    if(!planning_scene->getCollisionObjectMsg(pickup_goal.collision_object_name, 
                                              co)) {
      ROS_WARN_STREAM("Don't appear to have object " << pickup_goal.collision_object_name 
                      << " in planning scene, so can't tell where it is to pick it up");
      return;
    }
    in_object_frame = true;
    //ROS_DEBUG_STREAM("In object frame is true " << co.poses[0]);
    //TODO - Use potential pose?
    //Eigen::Affine3d potential_pose;
    //planning_models::poseFromMsg(pickup_goal.target.potential_models[0].pose.pose, potential_pose);
    // planning_scene->getTransforms()->transformPose(state,
    //                                                pickup_goal.target.potential_models[0].pose.header.frame_id,
    //                                                potential_pose,
    //                                                obj_pose);
    planning_models::poseFromMsg(co.mesh_poses[0], obj_pose);
  } 
  
  //assumes that whatever is in there is valid
  unsigned int current_size = execution_info.size();
  execution_info.pickup_goal_ = pickup_goal;
  execution_info.grasps_ = grasps;
  execution_info.resize(grasps.size());
  for(unsigned int i = current_size; i < execution_info.size(); i++) {
    execution_info[i].result_.result_code = 0;
  }

  Eigen::Vector3d pregrasp_dir(approach_direction.x,approach_direction.y,approach_direction.z);
  //tf::vector3MsgToTF(doNegate(handDescription().approachDirection(pickup_goal.arm_name)), pregrasp_dir);
  pregrasp_dir.normalize();

  Eigen::Vector3d lift_dir;
  lift_dir.x() = pickup_goal.lift.direction.vector.x;
  lift_dir.y() = pickup_goal.lift.direction.vector.y;
  lift_dir.z() = pickup_goal.lift.direction.vector.z;<commit_msg>Changing grasp evaluator to use either mesh poses or primitive poses<commit_after>/*********************************************************************
*
*  Copyright (c) 2009, <NAME>, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

// Author(s): <NAME>

#include <grasp_place_evaluation/grasp_evaluator_fast.h>

using moveit_manipulation_msgs::GraspResult;
using moveit_msgs::MoveItErrorCodes;

namespace grasp_place_evaluation {

GraspEvaluatorFast::GraspEvaluatorFast(const planning_models::KinematicModelConstPtr& kmodel,
                                       const std::map<std::string, kinematics::KinematicsBasePtr>& solver_map)
  : GraspEvaluator(), 
    InterpolationEvaluator(kmodel, solver_map)
{  
}

void GraspEvaluatorFast::testGrasps(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                    const planning_models::KinematicState* seed_state,
                                    const moveit_manipulation_msgs::PickupGoal& pickup_goal,
                                    const geometry_msgs::Vector3& approach_direction,
                                    const std::vector<moveit_manipulation_msgs::Grasp>& grasps,
                                    GraspExecutionInfoVector &execution_info,
                                    bool return_on_first_hit) 
  
{
  ros::WallTime start = ros::WallTime::now();

  std::map<unsigned int, unsigned int> outcome_count;
    
  planning_models::KinematicState state(*seed_state);

  std::map<std::string, double> planning_scene_state_values;
  state.getStateValues(planning_scene_state_values);

  std::string end_effector_group = planning_scene->getKinematicModel()->getJointModelGroup(pickup_goal.arm_name)->getAttachedEndEffectorGroupName();
  // links are ordered by the order seen by depth-first. This means that for a chain the last link is the tip of the chain
  std::string tip_link = planning_scene->getKinematicModel()->getJointModelGroup(pickup_goal.arm_name)->getLinkModelNames().back();
  
  std::vector<std::string> end_effector_links = planning_scene->getKinematicModel()->getJointModelGroup(end_effector_group)->getLinkModelNames();
  std::vector<std::string> arm_links = planning_scene->getKinematicModel()->getJointModelGroup(pickup_goal.arm_name)->getLinkModelNames();
  
  collision_detection::AllowedCollisionMatrix original_acm = planning_scene->getAllowedCollisionMatrix();
  collision_detection::AllowedCollisionMatrix group_disable_acm = original_acm;//planning_scene->disableCollisionsForNonUpdatedLinks(pickup_goal.arm_name);

  collision_detection::AllowedCollisionMatrix object_disable_acm = group_disable_acm;
  object_disable_acm.setEntry(pickup_goal.collision_object_name, end_effector_links, true); 
  collision_detection::AllowedCollisionMatrix object_support_disable_acm = object_disable_acm;
  if(!pickup_goal.collision_support_surface_name.empty()) {
    //will come into play when object is attached
    object_support_disable_acm.setEntry(pickup_goal.collision_support_surface_name, pickup_goal.collision_object_name, true); 
    if(pickup_goal.allow_gripper_support_collision)
    {
      if(pickup_goal.collision_support_surface_name == "\"all\"")
      {
        for(unsigned int i = 0; i < end_effector_links.size(); i++){
          object_support_disable_acm.setDefaultEntry(end_effector_links[i], true);
        }
      } else {
        object_support_disable_acm.setEntry(pickup_goal.collision_support_surface_name, end_effector_links, true); 
      }
    }
  }
  collision_detection::AllowedCollisionMatrix object_all_arm_disable_acm = object_disable_acm;
  collision_detection::AllowedCollisionMatrix object_support_all_arm_disable_acm = object_support_disable_acm;
  collision_detection::AllowedCollisionMatrix group_all_arm_disable_acm = group_disable_acm;

  //turning off collisions for the arm associated with this end effector
  for(unsigned int i = 0; i < arm_links.size(); i++) {
    object_all_arm_disable_acm.setDefaultEntry(arm_links[i], true);
    object_support_all_arm_disable_acm.setDefaultEntry(arm_links[i], true);
    group_all_arm_disable_acm.setDefaultEntry(arm_links[i], true);
  }
  
  //first we apply link padding for grasp check
  //cm->applyLinkPaddingToCollisionSpace(linkPaddingForGrasp(pickup_goal));
    
  //setup that's not grasp specific  
  std_msgs::Header target_header;
  target_header.frame_id = pickup_goal.target.reference_frame_id;

  bool in_object_frame = false;
  Eigen::Affine3d obj_pose(Eigen::Affine3d::Identity());

  if(pickup_goal.target.reference_frame_id == pickup_goal.collision_object_name) {
    moveit_msgs::CollisionObject co;
    if(!planning_scene->getCollisionObjectMsg(pickup_goal.collision_object_name, 
                                              co)) {
      ROS_WARN_STREAM("Don't appear to have object " << pickup_goal.collision_object_name 
                      << " in planning scene, so can't tell where it is to pick it up");
      return;
    }
    in_object_frame = true;
    //ROS_DEBUG_STREAM("In object frame is true " << co.poses[0]);
    //TODO - Use potential pose?
    //Eigen::Affine3d potential_pose;
    //planning_models::poseFromMsg(pickup_goal.target.potential_models[0].pose.pose, potential_pose);
    // planning_scene->getTransforms()->transformPose(state,
    //                                                pickup_goal.target.potential_models[0].pose.header.frame_id,
    //                                                potential_pose,
    //                                                obj_pose);
    if(co.mesh_poses.size() > 0) {
      planning_models::poseFromMsg(co.mesh_poses[0], obj_pose);
    } else if(co.primitive_poses.size() > 0) {
      planning_models::poseFromMsg(co.primitive_poses[0], obj_pose);
    } else {
      ROS_ERROR_STREAM("No poses for object");
      return;
    }
  } 
  
  //assumes that whatever is in there is valid
  unsigned int current_size = execution_info.size();
  execution_info.pickup_goal_ = pickup_goal;
  execution_info.grasps_ = grasps;
  execution_info.resize(grasps.size());
  for(unsigned int i = current_size; i < execution_info.size(); i++) {
    execution_info[i].result_.result_code = 0;
  }

  Eigen::Vector3d pregrasp_dir(approach_direction.x,approach_direction.y,approach_direction.z);
  //tf::vector3MsgToTF(doNegate(handDescription().approachDirection(pickup_goal.arm_name)), pregrasp_dir);
  pregrasp_dir.normalize();

  Eigen::Vector3d lift_dir;
  lift_dir.x() = pickup_goal.lift.direction.vector.x;
  lift_dir.y() = pickup_goal.lift.direction.vector.y;
  lift_dir.z() = pickup_goal.lift.direction.vector.z;
{'fraction_non_alphanumeric': 0.10256250777459883, 'fraction_numerical': 0.002425674835178505, 'mean_word_length': 4.042019441831295, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 59, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 32, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5324321', 'n_tokens_mistral': 4912, 'n_tokens_neox': 4579, 'n_words': 1018}