cleanup PlannerInterface

- remove group + timeout properties: they are passed as arguments to plan()
- move max_velocity_scaling_factor, max_acceleration_scaling_factor to PlannerInterface base class
This commit is contained in:
Robert Haschke 2018-06-13 00:09:10 +02:00
parent ba88c59201
commit 18bf5246ed
13 changed files with 53 additions and 19 deletions

View File

@ -47,9 +47,6 @@ class CartesianPath : public PlannerInterface {
public:
CartesianPath();
void setGroup(const std::string &group) { setProperty("group", group); }
void setTimeout(double timeout) { setProperty("timeout", timeout); }
void setStepSize(double step_size) { setProperty("step_size", step_size); }
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }

View File

@ -50,9 +50,6 @@ class JointInterpolationPlanner : public PlannerInterface {
public:
JointInterpolationPlanner();
void setGroup(const std::string &group) { setProperty("group", group); }
void setTimeout(double timeout) { setProperty("timeout", timeout); }
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from,

View File

@ -52,9 +52,7 @@ class PipelinePlanner : public PlannerInterface {
public:
PipelinePlanner();
void setGroup(const std::string &group) { setProperty("group", group); }
void setPlannerId(const std::string &planner) { setProperty("planner", planner); }
void setTimeout(double timeout) { setProperty("timeout", timeout); }
void init(const moveit::core::RobotModelConstPtr& robot_model) override;

View File

@ -63,7 +63,8 @@ class PlannerInterface {
PropertyMap properties_;
public:
PlannerInterface() {}
PlannerInterface();
virtual ~PlannerInterface() {}
PropertyMap& properties() { return properties_; }
const PropertyMap& properties() const { return properties_; }

View File

@ -26,6 +26,7 @@ add_library(${PROJECT_NAME}
storage.cpp
task.cpp
solvers/planner_interface.cpp
solvers/cartesian_path.cpp
solvers/pipeline_planner.cpp
solvers/joint_interpolation.cpp

View File

@ -48,8 +48,6 @@ CartesianPath::CartesianPath()
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
}
void CartesianPath::init(const core::RobotModelConstPtr &robot_model)

View File

@ -46,8 +46,6 @@ JointInterpolationPlanner::JointInterpolationPlanner()
{
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr &robot_model)

View File

@ -52,8 +52,6 @@ PipelinePlanner::PipelinePlanner()
p.declare<std::string>("planner", "", "planner id");
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<moveit_msgs::WorkspaceParameters>("workspace_parameters", moveit_msgs::WorkspaceParameters(), "allowed workspace of mobile base?");
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");

View File

@ -0,0 +1,50 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Bielefeld University
* 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 Bielefeld University 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.
*********************************************************************/
/* Authors: Robert Haschke
Desc: Planner Interface: implementation details shared across different planners
*/
#include <moveit/task_constructor/solvers/planner_interface.h>
namespace moveit { namespace task_constructor { namespace solvers {
PlannerInterface::PlannerInterface()
{
auto& p = properties();
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
}
} } }

View File

@ -99,7 +99,6 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
}
{
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto move = new MoveTo(forward ? "close gripper" : "open gripper", pipeline);

View File

@ -44,7 +44,6 @@ TEST(PA10, pick) {
t.setProperty("gripper", std::string("left_hand"));
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto cartesian = std::make_shared<solvers::CartesianPath>();

View File

@ -40,7 +40,6 @@ TEST(PR2, pick) {
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = {{"left_arm", pipeline}, {"left_gripper", pipeline}};

View File

@ -42,7 +42,6 @@ TEST(UR5, pick) {
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = {{"arm", pipeline}, {"gripper", pipeline}};