mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
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:
parent
ba88c59201
commit
18bf5246ed
@ -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); }
|
||||
|
@ -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,
|
||||
|
@ -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;
|
||||
|
||||
|
@ -63,7 +63,8 @@ class PlannerInterface {
|
||||
PropertyMap properties_;
|
||||
|
||||
public:
|
||||
PlannerInterface() {}
|
||||
PlannerInterface();
|
||||
virtual ~PlannerInterface() {}
|
||||
|
||||
PropertyMap& properties() { return properties_; }
|
||||
const PropertyMap& properties() const { return properties_; }
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
|
@ -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)
|
||||
|
@ -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");
|
||||
|
50
core/src/solvers/planner_interface.cpp
Normal file
50
core/src/solvers/planner_interface.cpp
Normal 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");
|
||||
}
|
||||
|
||||
} } }
|
@ -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);
|
||||
|
@ -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>();
|
||||
|
||||
|
@ -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}};
|
||||
|
@ -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}};
|
||||
|
Loading…
Reference in New Issue
Block a user