diff --git a/CMakeLists.txt b/CMakeLists.txt index fcada41..7215a1e 100644 --- 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 index ef06898..306a1bb 100644 --- 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 new file mode 100644 index 0000000..d19d9f0 --- /dev/null +++ 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 \ No newline at end of file diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index 0ac4cb0..f58c6b5 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -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 :