Skip to content

Commit

Permalink
Constraints Library Refactor (moveit#1428)
Browse files Browse the repository at this point in the history 
* Moved constraints library internal to the ModelBasedPlanningContext

In preparation to a larger change that moves much of the context initialization
internal to the ModelBasedPlanningContext as well.

* Removed unused OMPLPlannerService.

* Fixed tests since ModelBasedStateSpace is now virtual

* Corrected switched comments

* Fixed compile issues, and got construct database to work.

Also added launch file for ease of use.

* Only install the generate state db script to CATKIN_PACKAGE_BIN_DEST
  • Loading branch information
@BryceStevenWilley
BryceStevenWilley authored Nov 20, 2019
1 parent 683d655 commit cd202ab
Show file tree
Hide file tree
Showing 16 changed files with 285 additions and 424 deletions.
10 changes: 3 additions & 7 deletions moveit_planners/ompl/ompl_interface/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@ set(MOVEIT_LIB_NAME moveit_ompl_interface)
add_library(${MOVEIT_LIB_NAME}
src/ompl_interface.cpp
src/planning_context_manager.cpp
src/constraints_library.cpp
src/model_based_planning_context.cpp
src/parameterization/model_based_state_space.cpp
src/parameterization/model_based_state_space_factory.cpp
Expand All @@ -15,6 +14,7 @@ add_library(${MOVEIT_LIB_NAME}
src/detail/state_validity_checker.cpp
src/detail/projection_evaluators.cpp
src/detail/goal_union.cpp
src/detail/constraints_library.cpp
src/detail/constrained_sampler.cpp
src/detail/constrained_valid_state_sampler.cpp
src/detail/constrained_goal_sampler.cpp
Expand All @@ -27,11 +27,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} $
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")

add_executable(moveit_ompl_planner src/ompl_planner.cpp)
target_link_libraries(moveit_ompl_planner ${MOVEIT_LIB_NAME})
set_target_properties(moveit_ompl_planner PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")

add_executable(moveit_generate_state_database src/generate_state_database.cpp)
add_executable(moveit_generate_state_database scripts/generate_state_database.cpp)
target_link_libraries(moveit_generate_state_database ${MOVEIT_LIB_NAME})
set_target_properties(moveit_generate_state_database PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
set_target_properties(moveit_generate_state_database PROPERTIES OUTPUT_NAME "generate_state_database")
Expand All @@ -44,7 +40,7 @@ install(TARGETS ${MOVEIT_LIB_NAME} moveit_ompl_planner_plugin
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(TARGETS moveit_ompl_planner moveit_generate_state_database
install(TARGETS moveit_generate_state_database
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})

Expand Down
15 changes: 8 additions & 7 deletions ...veit/ompl_interface/constraints_library.h → ...pl_interface/detail/constraints_library.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@
#pragma once

#include <moveit/macros/class_forward.h>
#include <moveit/ompl_interface/planning_context_manager.h>
#include <moveit/ompl_interface/model_based_planning_context.h>
#include <moveit/kinematic_constraints/kinematic_constraint.h>
#include <ompl/base/StateStorage.h>
#include <boost/serialization/map.hpp>
Expand Down Expand Up @@ -160,7 +160,7 @@ MOVEIT_CLASS_FORWARD(ConstraintsLibrary);
class ConstraintsLibrary
{
public:
ConstraintsLibrary(const PlanningContextManager& pcontext) : context_manager_(pcontext)
ConstraintsLibrary(ModelBasedPlanningContext* pcontext) : context_(pcontext)
{
}

Expand Down Expand Up @@ -190,12 +190,13 @@ class ConstraintsLibrary
const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::Constraints& msg) const;

private:
ompl::base::StateStoragePtr constructConstraintApproximation(
const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
ConstraintApproximationConstructionResults& result);
ompl::base::StateStoragePtr
constructConstraintApproximation(ModelBasedPlanningContext* pcontext, const moveit_msgs::Constraints& constr_sampling,
const moveit_msgs::Constraints& constr_hard,
const ConstraintApproximationConstructionOptions& options,
ConstraintApproximationConstructionResults& result);

const PlanningContextManager& context_manager_;
ModelBasedPlanningContext* context_;
std::map<std::string, ConstraintApproximationPtr> constraint_approximations_;
};
}
27 changes: 23 additions & 4 deletions ...planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,6 @@ struct ModelBasedPlanningContextSpecification
{
std::map<std::string, std::string> config_;
ConfiguredPlannerSelector planner_selector_;
ConstraintsLibraryConstPtr constraints_library_;
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_;

ModelBasedStateSpacePtr state_space_;
Expand Down Expand Up @@ -241,9 +240,19 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext
const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error);
bool setPathConstraints(const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error);

void setConstraintsApproximations(const ConstraintsLibraryConstPtr& constraints_library)
void setConstraintsApproximations(const ConstraintsLibraryPtr& constraints_library)
{
spec_.constraints_library_ = constraints_library;
constraints_library_ = constraints_library;
}

ConstraintsLibraryPtr getConstraintsLibraryNonConst()
{
return constraints_library_;
}

const ConstraintsLibraryPtr& getConstraintsLibrary() const
{
return constraints_library_;
}

bool useStateValidityCache() const
Expand Down Expand Up @@ -304,7 +313,15 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext

void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const;

virtual void configure();
/** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint
* approximations to */
bool loadConstraintApproximations(const ros::NodeHandle& nh);

/** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint
* approximations to */
bool saveConstraintApproximations(const ros::NodeHandle& nh);

virtual void configure(const ros::NodeHandle& nh, bool use_constraints_approximations);

protected:
void preSolve();
Expand Down Expand Up @@ -373,6 +390,8 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext

bool use_state_validity_cache_;

ConstraintsLibraryPtr constraints_library_;

bool simplify_solutions_;
};
} // namespace ompl_interface
28 changes: 0 additions & 28 deletions moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
#pragma once

#include <moveit/ompl_interface/planning_context_manager.h>
#include <moveit/ompl_interface/constraints_library.h>
#include <moveit/constraint_samplers/constraint_sampler_manager.h>
#include <moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h>
#include <moveit/planning_interface/planning_interface.h>
Expand Down Expand Up @@ -86,9 +85,6 @@ class OMPLInterface
const planning_interface::MotionPlanRequest& req,
moveit_msgs::MoveItErrorCodes& error_code) const;

ModelBasedPlanningContextPtr getPlanningContext(const std::string& config,
const std::string& factory_type = "") const;

const PlanningContextManager& getPlanningContextManager() const
{
return context_manager_;
Expand All @@ -99,16 +95,6 @@ class OMPLInterface
return context_manager_;
}

ConstraintsLibrary& getConstraintsLibrary()
{
return *constraints_library_;
}

const ConstraintsLibrary& getConstraintsLibrary() const
{
return *constraints_library_;
}

constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager()
{
return *constraint_sampler_manager_;
Expand All @@ -128,11 +114,6 @@ class OMPLInterface
{
return use_constraints_approximations_;
}

void loadConstraintApproximations(const std::string& path);

void saveConstraintApproximations(const std::string& path);

bool simplifySolutions() const
{
return simplify_solutions_;
Expand All @@ -143,14 +124,6 @@ class OMPLInterface
simplify_solutions_ = flag;
}

/** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint
* approximations to */
bool saveConstraintApproximations();

/** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint
* approximations to */
bool loadConstraintApproximations();

/** @brief Print the status of this node*/
void printStatus();

Expand Down Expand Up @@ -183,7 +156,6 @@ class OMPLInterface

PlanningContextManager context_manager_;

ConstraintsLibraryPtr constraints_library_;
bool use_constraints_approximations_;

bool simplify_solutions_;
Expand Down
5 changes: 5 additions & 0 deletions ...face/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,5 +46,10 @@ class JointModelStateSpace : public ModelBasedStateSpace
static const std::string PARAMETERIZATION_TYPE;

JointModelStateSpace(const ModelBasedStateSpaceSpecification& spec);

const std::string& getParameterizationType() const override
{
return PARAMETERIZATION_TYPE;
}
};
}
2 changes: 2 additions & 0 deletions ...l/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,6 +200,8 @@ class ModelBasedStateSpace : public ompl::base::StateSpace

ompl::base::StateSamplerPtr allocDefaultStateSampler() const override;

virtual const std::string& getParameterizationType() const = 0;

const robot_model::RobotModelConstPtr& getRobotModel() const
{
return spec_.robot_model_;
Expand Down
5 changes: 5 additions & 0 deletions ...erface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,11 @@ class PoseModelStateSpace : public ModelBasedStateSpace
void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const override;
void sanityChecks() const override;

const std::string& getParameterizationType() const override
{
return PARAMETERIZATION_TYPE;
}

private:
struct PoseComponent
{
Expand Down
6 changes: 2 additions & 4 deletions moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,12 +140,10 @@ class PlanningContextManager
return robot_model_;
}

ModelBasedPlanningContextPtr getPlanningContext(const std::string& config,
const std::string& factory_type = "") const;

ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
moveit_msgs::MoveItErrorCodes& error_code) const;
moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh,
bool use_constraints_approximations) const;

void registerPlannerAllocator(const std::string& planner_id, const ConfiguredPlannerAllocator& pa)
{
Expand Down
14 changes: 14 additions & 0 deletions moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<launch>
<arg name="use_current_scene" default="false"/>
<arg name="planning_group"/>
<arg name="constraints_file" doc="the path to a constraints yaml file (see generate_state_database for details)"/>

<node name="generate_state_database" pkg="moveit_planners_ompl" type="generate_state_database" output="screen">
<param name="use_current_scene" value="$(arg use_current_scene)"/>
<param name="planning_group" value="$(arg planning_group)"/>
<remap from="~output_folder" to="move_group/constraint_approximations_path"/>

<rosparam ns="constraints" command="load" file="$(arg constraints_file)"/>
</node>

</launch>
29 changes: 26 additions & 3 deletions ...interface/src/generate_state_database.cpp → ...rface/scripts/generate_state_database.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,12 @@
/* Authors: Ioan Sucan, Michael Goerner */

#include <moveit/ompl_interface/ompl_interface.h>
#include <moveit/ompl_interface/detail/constraints_library.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/profiler/profiler.h>

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/conversions.h>

#include <moveit/kinematic_constraints/utils.h>
#include <moveit/utils/message_checks.h>
Expand Down Expand Up @@ -119,26 +121,47 @@ void computeDB(const planning_scene::PlanningScenePtr& scene, struct GenerateSta
scene->getCurrentStateNonConst().update();

ompl_interface::OMPLInterface ompl_interface(scene->getRobotModel());
planning_interface::MotionPlanRequest req;
req.group_name = params.planning_group;
req.path_constraints = params.constraints;
moveit::core::robotStateToRobotStateMsg(scene->getCurrentState(), req.start_state);
req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints(
scene->getCurrentState(), scene->getRobotModel()->getJointModelGroup(params.planning_group)));

ompl_interface::ModelBasedPlanningContextPtr context = ompl_interface.getPlanningContext(scene, req);

ROS_INFO_STREAM_NAMED(LOGNAME, "Generating Joint Space Constraint Approximation Database for constraint:\n"
<< params.constraints);

ompl_interface::ConstraintApproximationConstructionResults result =
ompl_interface.getConstraintsLibrary().addConstraintApproximation(params.constraints, params.planning_group,
scene, params.construction_opts);
context->getConstraintsLibraryNonConst()->addConstraintApproximation(params.constraints, params.planning_group,
scene, params.construction_opts);

if (!result.approx)
{
ROS_FATAL_NAMED(LOGNAME, "Failed to generate approximation.");
return;
}
ompl_interface.getConstraintsLibrary().saveConstraintApproximations(params.output_folder);
context->getConstraintsLibraryNonConst()->saveConstraintApproximations(params.output_folder);
ROS_INFO_STREAM_NAMED(LOGNAME,
"Successfully generated Joint Space Constraint Approximation Database for constraint:\n"
<< params.constraints);
ROS_INFO_STREAM_NAMED(LOGNAME, "The database has been saved in your local folder '" << params.output_folder << "'");
}

/**
* Generates a database of states that follow the given constraints.
* An example of the constraint yaml that should be loaded to rosparam:
* "name: tool0_upright
* constraints:
* - type: orientation
* frame_id: world
* link_name: tool0
* orientation: [0, 0, 0]
* tolerances: [0.01, 0.01, 3.15]
* weight: 1.0
* "
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "construct_tool_constraint_database", ros::init_options::AnonymousName);
Expand Down
Loading

0 comments on commit cd202ab

Please sign in to comment.