WIP Expose an interface for passing in a function to translate joint values to grasp width

include/moveit_grasps/grasp_candidate.h

@@ -175,6 +175,6 @@ class GraspCandidate
 
 typedef std::shared_ptr<GraspCandidate> GraspCandidatePtr;
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/grasp_data.h

@@ -136,6 +136,6 @@ struct GraspData
   double grasp_padding_on_approach_;
 };
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/grasp_filter.h

@@ -386,6 +386,6 @@ class GraspFilter
 typedef std::shared_ptr<GraspFilter> GraspFilterPtr;
 typedef std::shared_ptr<const GraspFilter> GraspFilterConstPtr;
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/grasp_generator.h

@@ -243,6 +243,6 @@ class GraspGenerator
 typedef std::shared_ptr<GraspGenerator> GraspGeneratorPtr;
 typedef std::shared_ptr<const GraspGenerator> GraspGeneratorConstPtr;
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/grasp_planner.h

@@ -158,6 +158,6 @@ class GraspPlanner
 typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr;
 typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr;
 
-}  // end namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/suction_grasp_filter.h

@@ -84,11 +84,8 @@ class SuctionGraspFilter : public GraspFilter
    */
   bool filterGraspsBySuctionVoxelOverlap(std::vector<GraspCandidatePtr>& grasp_candidates);
 
-  [[deprecated("Use filterGraspsBySuctionVoxelOverlap")]] bool
-  filterGraspsBySuctionVoxelOverlapCutoff(std::vector<GraspCandidatePtr>& grasp_candidates)
-  {
-    return filterGraspsBySuctionVoxelOverlap(grasp_candidates);
-  }
+  [[deprecated("Use filterGraspsBySuctionVoxelOverlap")]] bool filterGraspsBySuctionVoxelOverlapCutoff(
+      std::vector<GraspCandidatePtr>& grasp_candidates) { return filterGraspsBySuctionVoxelOverlap(grasp_candidates); }
 
   /**
    * \brief  For suction grippers, set the cutoff threshold used by preFilterBySuctionVoxelOverlap to

include/moveit_grasps/suction_grasp_generator.h

@@ -92,8 +92,7 @@ class SuctionGraspGenerator : public GraspGenerator
   [[deprecated("Object_width no longer needs to be specified for suction grasps")]] bool
   addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, const SuctionGraspDataPtr& grasp_data,
            const Eigen::Isometry3d& object_pose, const Eigen::Vector3d& object_size, double object_width,
-           std::vector<GraspCandidatePtr>& grasp_candidates)
-  {
+           std::vector<GraspCandidatePtr>& grasp_candidates) {
     return addGrasp(grasp_pose_eef_mount, grasp_data, object_pose, object_size, grasp_candidates);
   }
 
@@ -146,6 +145,6 @@ class SuctionGraspGenerator : public GraspGenerator
 typedef std::shared_ptr<SuctionGraspGenerator> SuctionGraspGeneratorPtr;
 typedef std::shared_ptr<const SuctionGraspGenerator> SuctionGraspGeneratorConstPtr;
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/suction_voxel_matrix.h

@@ -183,6 +183,6 @@ class SuctionVoxelMatrix
   std::vector<std::vector<std::shared_ptr<SuctionVoxel>>> suction_voxels_;
 };
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/two_finger_grasp_data.h

@@ -55,9 +55,13 @@ class TwoFingerGraspData : public GraspData
    * \param node_handle - allows for namespacing
    * \param end_effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names
    * \param robot_model - The robot model
+   * TODO add param descripton function
    */
   TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector,
-                     const moveit::core::RobotModelConstPtr& robot_model);
+                     const moveit::core::RobotModelConstPtr& robot_model,
+                     const std::function<std::vector<double>(double, double, double, const std::vector<std::string>&,
+                                                             const std::vector<double>&, const std::vector<double>&)>&
+                         get_joint_positions_from_width_func = nullptr);
 
   /**
    * \brief Helper function for constructor, loads grasp data from a yaml file (load from roslaunch)
@@ -99,6 +103,13 @@ class TwoFingerGraspData : public GraspData
    */
   void print() override;
 
+private:
+  static std::vector<double> getJointPositionsFromWidthDefault(double distance_btw_fingers, double max_finger_width_,
+                                                               double min_finger_width_,
+                                                               const std::vector<std::string>& joint_names,
+                                                               const std::vector<double>& grasp_pose,
+                                                               const std::vector<double>& pre_grasp_pose);
+
 public:
   /////////////////////////////////////
   // Finger gripper specific parameters
@@ -113,8 +124,13 @@ class TwoFingerGraspData : public GraspData
   double min_finger_width_;
   // Parameter used to ensure generated grasps will overlap object
   double gripper_finger_width_;
+
+  // a function to translate from joint values to grasp width
+  std::function<std::vector<double>(double, double, double, const std::vector<std::string>&, const std::vector<double>&,
+                                    const std::vector<double>&)>
+      get_joint_positions_from_width_func;
 };
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/two_finger_grasp_filter.h

@@ -76,6 +76,6 @@ class TwoFingerGraspFilter : public GraspFilter
 typedef std::shared_ptr<TwoFingerGraspFilter> TwoFingerGraspFilterPtr;
 typedef std::shared_ptr<const TwoFingerGraspFilter> TwoFingerGraspFilterConstPtr;
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

include/moveit_grasps/two_finger_grasp_generator.h

@@ -252,6 +252,6 @@ class TwoFingerGraspGenerator : public GraspGenerator
 typedef std::shared_ptr<TwoFingerGraspGenerator> TwoFingerGraspGeneratorPtr;
 typedef std::shared_ptr<const TwoFingerGraspGenerator> TwoFingerGraspGeneratorConstPtr;
 
-}  // namespace
+}  // namespace moveit_grasps
 
 #endif

src/demo/grasp_filter_demo.cpp

@@ -284,7 +284,7 @@ class GraspFilterDemo
 
 };  // end of class
 
-}  // namespace
+}  // namespace moveit_grasps_demo
 
 int main(int argc, char* argv[])
 {

src/demo/grasp_generator_demo.cpp

@@ -136,8 +136,10 @@ class GraspGeneratorDemo
     grasp_visuals_->publishAxisLabeled(grasp_generator_->ideal_grasp_pose_, "IDEAL_TCP_GRASP_POSE");
     visual_tools_->publishEEMarkers(grasp_generator_->ideal_grasp_pose_ * grasp_data_->tcp_to_eef_mount_, ee_jmg,
                                     grasp_data_->grasp_posture_.points[0].positions, rviz_visual_tools::BLUE);
-    grasp_visuals_->publishAxisLabeled(grasp_generator_->ideal_grasp_pose_ * grasp_data_->tcp_to_eef_mount_,
-                                       "IDEAL EEF MOUNT POSE");
+    grasp_visuals_->publishAxisLabeled(grasp_generator_->ideal_grasp_pose_ * grasp_data_->tcp_to_eef_mount_, "IDEAL "
+                                                                                                             "EEF "
+                                                                                                             "MOUNT "
+                                                                                                             "POSE");
     visual_tools_->trigger();
 
     // ---------------------------------------------------------------------------------------------
@@ -283,7 +285,7 @@ class GraspGeneratorDemo
 
 };  // end of class
 
-}  // namespace
+}  // namespace moveit_grasps_demo
 
 int main(int argc, char* argv[])
 {

src/demo/grasp_pipeline_demo.cpp

@@ -79,7 +79,7 @@ void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_too
   visual_tools->prompt(prompt);
 }
 
-}  // end annonymous namespace
+}  // namespace
 
 class GraspPipelineDemo
 {
@@ -503,7 +503,7 @@ class GraspPipelineDemo
 
 };  // end of class
 
-}  // namespace
+}  // namespace moveit_grasps_demo
 
 int main(int argc, char* argv[])
 {

src/demo/suction_grasp_pipeline_demo.cpp

@@ -80,7 +80,7 @@ void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_too
   visual_tools->prompt(prompt);
 }
 
-}  // end annonymous namespace
+}  // namespace
 
 class GraspPipelineDemo
 {
@@ -489,7 +489,7 @@ class GraspPipelineDemo
 
 };  // end of class
 
-}  // namespace
+}  // namespace moveit_grasps_demo
 
 int main(int argc, char* argv[])
 {

src/grasp_candidate.cpp

@@ -113,4 +113,4 @@ bool GraspCandidate::isValid()
   return grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED;
 }
 
-}  // namespace
+}  // namespace moveit_grasps

src/grasp_data.cpp

@@ -150,14 +150,16 @@ bool GraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_
     state.update();
     if (!state.knowsFrameTransform(parent_link_->getName()))
     {
-      ROS_ERROR_NAMED("grasp_data", "Robot Model does not know the frame transform for the end effector group parent "
-                                    "frame: %s. Did you set a parent link in the srdf?",
+      ROS_ERROR_NAMED("grasp_data",
+                      "Robot Model does not know the frame transform for the end effector group parent "
+                      "frame: %s. Did you set a parent link in the srdf?",
                       parent_link_->getName().c_str());
     }
     if (!state.knowsFrameTransform(tcp_name_))
     {
-      ROS_ERROR_NAMED("grasp_data", "Robot Model does not know the frame transform for the tcp frame: %s. Is it "
-                                    "available in the urdf?",
+      ROS_ERROR_NAMED("grasp_data",
+                      "Robot Model does not know the frame transform for the tcp frame: %s. Is it "
+                      "available in the urdf?",
                       tcp_name_.c_str());
     }
     Eigen::Isometry3d eef_mount_pose = state.getGlobalLinkTransform(parent_link_);
@@ -207,4 +209,4 @@ void GraspData::print()
   std::cout << "grasp_padding_on_approach_: " << grasp_padding_on_approach_ << std::endl;
 }
 
-}  // namespace
+}  // namespace moveit_grasps

src/grasp_filter.cpp

@@ -878,4 +878,4 @@ void GraspFilter::publishPlanningScene(const planning_scene::PlanningScenePtr& p
   planning_scene_publisher_.publish(msg);
 }
 
-}  // namespace
+}  // namespace moveit_grasps

src/grasp_planner.cpp

@@ -1,36 +1,36 @@
 /*********************************************************************
-   * Software License Agreement (BSD License)
-   *
-   *  Copyright (c) 2015, University of Colorado, Boulder
-   *  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 Univ of CO, Boulder 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.
-   *********************************************************************/
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  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 Univ of CO, Boulder 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: Dave Coleman <[email protected]>
    Desc:   Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists)
@@ -383,8 +383,8 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida
 
   if (!valid_path_found)
   {
-    ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "UNABLE to find valid waypoint cartesian path after "
-                                                          << MAX_IK_ATTEMPTS << " attempts");
+    ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints",
+                           "UNABLE to find valid waypoint cartesian path after " << MAX_IK_ATTEMPTS << " attempts");
     return false;
   }
 
@@ -432,4 +432,4 @@ bool GraspPlanner::isEnabled(const std::string& setting_name)
   return false;
 }
 
-}  // end namespace
+}  // namespace moveit_grasps

src/suction_grasp_data.cpp

@@ -108,4 +108,4 @@ void SuctionGraspData::print()
   std::cout << "\tactive_suction_range_y_: " << suction_voxel_matrix_->getActiveSuctionWidthX() << std::endl;
 }
 
-}  // namespace
+}  // namespace moveit_grasps

src/suction_grasp_filter.cpp

@@ -445,4 +445,4 @@ void SuctionGraspFilter::setSuctionVoxelOverlapCutoff(double cutoff)
   suction_voxel_overlap_cutoff_ = cutoff;
 }
 
-}  // namespace
+}  // namespace moveit_grasps

src/suction_grasp_generator.cpp

@@ -294,8 +294,8 @@ bool SuctionGraspGenerator::generateSuctionGrasps(const Eigen::Isometry3d& cuboi
 
   if (debug_top_grasps_)
   {
-    ROS_DEBUG_STREAM_NAMED("grasp_generator", "\n\tWidth:\t" << width << "\n\tDepth:\t" << depth << "\n\tHeight\t"
-                                                             << height);
+    ROS_DEBUG_STREAM_NAMED("grasp_generator",
+                           "\n\tWidth:\t" << width << "\n\tDepth:\t" << depth << "\n\tHeight\t" << height);
     visual_tools_->publishAxisLabeled(cuboid_top_pose, "cuboid_top_pose", rviz_visual_tools::SMALL);
     double suction_z_range = grasp_data->grasp_max_depth_ - grasp_data->grasp_min_depth_;
     visual_tools_->publishWireframeCuboid(cuboid_top_pose * Eigen::Translation3d(0, 0, suction_z_range / 2.0), depth,
@@ -418,8 +418,9 @@ bool SuctionGraspGenerator::generateSuctionGrasps(const Eigen::Isometry3d& cuboi
   if (!grasp_candidates.size())
     ROS_WARN_STREAM_NAMED("grasp_generator.generate_suction_grasps", "Generated 0 grasps");
   else
-    ROS_INFO_STREAM_NAMED("grasp_generator.generate_suction_grasps", "Generated " << grasp_candidates.size() << " grasp"
-                                                                                                                "s");
+    ROS_INFO_STREAM_NAMED("grasp_generator.generate_suction_grasps", "Generated " << grasp_candidates.size()
+                                                                                  << " grasp"
+                                                                                     "s");
 
   // Visualize animated grasps that have been generated
   if (show_prefiltered_grasps_)

src/suction_grasp_scorer.cpp

@@ -229,8 +229,8 @@ double SuctionGraspScorer::scoreSuctionVoxelOverlap(const Eigen::Isometry3d& gra
       }
     }
     // normalize
-    ROS_DEBUG_STREAM_NAMED("grasp_scorer.voxels.score", "overlap_vector[" << voxel_id
-                                                                          << "]     = " << overlap_vector[voxel_id]);
+    ROS_DEBUG_STREAM_NAMED("grasp_scorer.voxels.score",
+                           "overlap_vector[" << voxel_id << "]     = " << overlap_vector[voxel_id]);
 
     if (visual_tools)
     {