Merge pull request #31 from iretiayo/annealing_params

adding functionality to set the simulated annealing parameters
graspit-simulator · Feb 16, 2018 · 9a66677 · 9a66677
2 parents c716a50 + c7cf201
commit 9a66677
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -62,6 +62,7 @@ find_package(graspit)
    SearchContact.msg
    Energy.msg
    Contact.msg
+   SimAnnParams.msg
  )
 
 ## Generate services in the 'srv' folder

diff --git a/action/PlanGrasps.action b/action/PlanGrasps.action
@@ -5,6 +5,7 @@ Planner planner
 string search_energy
 SearchSpace search_space
 SearchContact search_contact
+SimAnnParams sim_ann_params
 
 int32 max_steps
 int32 feedback_num_steps

diff --git a/msg/SimAnnParams.msg b/msg/SimAnnParams.msg
@@ -0,0 +1,22 @@
+
+# flag to switch to custom params defined in this message. If not set, GraspIt's default settings are kept
+bool set_custom_params
+
+
+# //Annealing parameters
+# //! Annealing constant for neighbor generation schedule
+float64 YC	 	#	GraspIt! default: 7.0
+# //! Annealing constant for error acceptance schedule
+float64 HC	 	#	GraspIt! default: 7.0
+# //! Number of dimensions for neighbor generation schedule
+float64 YDIMS	#	GraspIt! default: 8.0
+# //! Number of dimensions for error acceptance schedule
+float64 HDIMS	#	GraspIt! default: 8.0
+# //! Adjust factor for neighbor generation schedule
+float64 NBR_ADJ	#	GraspIt! default: 1.0
+# //! Adjust raw errors reported by states to be in the relevant range of the annealing schedule
+float64 ERR_ADJ	#	GraspIt! default: 1.0e-6
+# //! Starting temperatue
+float64 DEF_T0	#	GraspIt! default: 1e6
+# //! Starting step
+float64 DEF_K0	#	GraspIt! default: 30000
diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp
@@ -13,6 +13,7 @@
 #include <graspit/EGPlanner/searchState.h>
 #include <graspit/EGPlanner/egPlanner.h>
 #include <graspit/EGPlanner/simAnnPlanner.h>
+#include <graspit/EGPlanner/simAnnParams.h>
 #include <graspit/EGPlanner/guidedPlanner.h>
 #include <graspit/EGPlanner/energy/searchEnergyFactory.h>
 #include <graspit/EGPlanner/energy/searchEnergy.h>
@@ -887,6 +888,20 @@ void GraspitInterface::runPlannerInMainThread()
 
     mPlanner->setEnergyType(goal.search_energy);
 
+    if (goal.sim_ann_params.set_custom_params)
+    {
+        ROS_INFO("Switching SimAnn Annealing parameters to your custom defined values!!! ");
+        SimAnnParams simAnnParams;
+        simAnnParams.YC = goal.sim_ann_params.YC;
+        simAnnParams.HC = goal.sim_ann_params.HC;
+        simAnnParams.YDIMS = goal.sim_ann_params.YDIMS;
+        simAnnParams.NBR_ADJ = goal.sim_ann_params.NBR_ADJ;
+        simAnnParams.ERR_ADJ = goal.sim_ann_params.ERR_ADJ;
+        simAnnParams.DEF_T0 = goal.sim_ann_params.DEF_T0;
+        simAnnParams.DEF_K0 = goal.sim_ann_params.DEF_K0;
+        mPlanner->setAnnealingParameters(simAnnParams);
+    }
+
 
     switch(goal.search_contact.type) {
         case graspit_interface::SearchContact::CONTACT_PRESET :