Merge branches 'master' 'generate-pose' and 'connect'

This commit is contained in:
Robert Haschke 2018-04-09 21:08:45 +02:00
commit 70065c98e7
24 changed files with 942 additions and 242 deletions

View File

@ -73,16 +73,10 @@ int main(int argc, char** argv){
stage->setMaxPenetration(0.1);
t.add(std::move(stage));
}
{
auto move = std::make_unique<stages::MoveTo>("open gripper", pipeline);
move->restrictDirection(stages::MoveTo::FORWARD);
move->properties().property("group").configureInitFrom(Stage::PARENT, "gripper");
move->setGoal("open");
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::Connect>("move to object", pipeline);
stages::Connect::GroupPlannerVector planners = {{"left_hand", pipeline}, {"left_arm", pipeline}};
auto move = std::make_unique<stages::Connect>("connect", planners);
move->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(move));
}

View File

@ -3,10 +3,10 @@
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/task_constructor/stages/pick.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
#include <shape_msgs/SolidPrimitive.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
@ -45,6 +45,17 @@ int main(int argc, char** argv){
initial_stage = initial.get();
t.add(std::move(initial));
// 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}};
auto connect = std::make_unique<stages::Connect>("connect", planners);
connect->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(connect));
// grasp generator
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
grasp_generator->setAngleDelta(.2);
@ -52,6 +63,7 @@ int main(int argc, char** argv){
grasp_generator->setGraspPose("closed");
grasp_generator->setMonitoredStage(initial_stage);
// pick stage
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
pick->setProperty("eef", std::string("left_gripper"));
pick->setProperty("object", std::string("object"));

View File

@ -107,14 +107,11 @@ public:
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
/// container used to represent a serial solution
typedef std::vector<const SolutionBase*> solution_container;
protected:
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase &s) override;
typedef std::function<void(const solution_container& trace,
typedef std::function<void(const SolutionSequence::container_type &trace,
double trace_accumulated_cost)> SolutionProcessor;
/// Traverse all solution pathes starting at start and going in given direction dir
@ -123,7 +120,7 @@ protected:
/// the full trace (from start to end, but not including start) and its accumulated costs
template<Interface::Direction dir>
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
solution_container &trace, double trace_cost = 0);
SolutionSequence::container_type &trace, double trace_cost = 0);
protected:
SerialContainer(SerialContainerPrivate* impl);

View File

@ -136,27 +136,6 @@ protected:
};
PIMPL_FUNCTIONS(ContainerBase)
/** Representation of a single, full solution path of a SerialContainer.
*
* A serial solution describes a full solution path through all children
* of a SerialContainer. This is a vector (of children().size()) of pointers
* to all solutions of the children. Hence, we don't need to copy those solutions. */
class SerialSolution : public SolutionBase {
public:
explicit SerialSolution(StagePrivate* creator, SerialContainer::solution_container&& subsolutions, double cost)
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override;
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
private:
/// series of sub solutions
SerialContainer::solution_container subsolutions_;
};
/* A solution of a SerialContainer needs to connect start to end via a full path.
* The solution of a single child stage is usually disconnected to the container's start or end.
@ -188,7 +167,7 @@ private:
InterfaceFlags accepted);
// set of all solutions
ordered<SerialSolution> solutions_;
ordered<SolutionSequence> solutions_;
};
PIMPL_FUNCTIONS(SerialContainer)

View File

@ -0,0 +1,69 @@
/*********************************************************************
* 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: Luca Lach, Robert Haschke */
#pragma once
#include <moveit/task_constructor/storage.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
namespace moveit { namespace task_constructor {
/// create a new JointModelGroup comprising all joints of the given groups
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
/** find duplicate, non-fixed joints
*
* Merging is only allowed for disjoint joint sets. Fixed joints are tolerated.
* Assumes that \e joints is the the union of the joint sets of all \e groups (w/o duplicates).
* The list of duplicate joints is returned in \e duplicates and in \e names (as a comma-separated list) */
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
std::vector<const moveit::core::JointModel*> joints,
std::vector<const moveit::core::JointModel*>& duplicates,
std::string& names);
/** merge all sub trajectories into a single RobotTrajectory for parallel execution
*
* As the RobotTrajectory maintains a pointer to the underlying JointModelGroup
* (to know about the involved joint names), a merged JointModelGroup needs to be passed
* or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory.
* For now, only the trajectory path is considered. Timings, velocities, etc. are ignored.
*/
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
core::JointModelGroup*& merged_group);
} }

View File

@ -339,6 +339,9 @@ protected:
class ConnectingPrivate;
class Connecting : public ComputeBase {
protected:
virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const;
public:
PRIVATE_CLASS(Connecting)
Connecting(const std::string& name);
@ -346,13 +349,13 @@ public:
void reset() override;
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
void connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory&& trajectory);
void connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory&& trajectory, double cost) {
void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory);
void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) {
trajectory.setCost(cost);
connect(from, to, std::move(trajectory));
}
void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
};
} }

View File

@ -103,7 +103,7 @@ public:
inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; }
void composePropertyErrorMsg(const std::string& name, std::ostream& os);
void newSolution(const SolutionBase &current);
void newSolution(SolutionBase& current);
protected:
Stage* const me_; // associated/owning Stage instance
@ -235,6 +235,10 @@ class ConnectingPrivate : public ComputeBasePrivate {
return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
}
};
template<Interface::Direction other>
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
public:
inline ConnectingPrivate(Connecting *me, const std::string &name);
@ -247,8 +251,8 @@ public:
private:
// get informed when new start or end state becomes available
void newStartState(Interface::iterator it, bool updated);
void newEndState(Interface::iterator it, bool updated);
template<Interface::Direction other>
void newState(Interface::iterator it, bool updated);
// ordered list of pending state pairs
ordered<StatePair, StatePairLess> pending;

View File

@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke
/* Authors: Robert Haschke, Michael Goerner
Desc: Connect arbitrary states by motion planning
*/
@ -46,8 +46,12 @@
namespace moveit { namespace task_constructor { namespace stages {
class Connect : public Connecting {
protected:
bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override;
public:
Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
Connect(std::string name, const GroupPlannerVector& planners);
void setTimeout(const ros::Duration& timeout){
setProperty("timeout", timeout.toSec());
@ -57,11 +61,27 @@ public:
setProperty("path_constraints", std::move(path_constraints));
}
void init(const moveit::core::RobotModelConstPtr& robot_model);
bool compute(const InterfaceState &from, const InterfaceState &to);
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool compute(const InterfaceState &from, const InterfaceState &to) override;
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override;
protected:
solvers::PlannerInterfacePtr planner_;
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
protected:
GroupPlannerVector planner_;
moveit::core::JointModelGroupPtr merged_jmg_;
// TODO: ComputeBase should handle any SolutionBase -> shared_ptr
std::list<SubTrajectory> subsolutions_;
std::list<SolutionSequence> solutions_;
std::list<InterfaceState> states_;
};
} } }

View File

@ -38,16 +38,15 @@
#pragma once
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/stages/generate_pose.h>
namespace moveit { namespace task_constructor { namespace stages {
class GenerateGraspPose : public MonitoringGenerator {
class GenerateGraspPose : public GeneratePose {
public:
GenerateGraspPose(const std::string& name);
void reset() override;
bool canCompute() const override;
void init(const core::RobotModelConstPtr &robot_model) override;
bool compute() override;
void setEndEffector(const std::string &eef);
@ -57,10 +56,6 @@ public:
protected:
void onNewSolution(const SolutionBase& s) override;
protected:
planning_scene::PlanningScenePtr scene_;
double current_angle_ = 0.0;
};
} } }

View File

@ -0,0 +1,64 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Hamburg 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: Michael Goerner
Desc: Generator Stage for poses
*/
#pragma once
#include <moveit/task_constructor/stage.h>
#include <geometry_msgs/PoseStamped.h>
namespace moveit { namespace task_constructor { namespace stages {
class GeneratePose : public MonitoringGenerator {
public:
GeneratePose(const std::string& name);
void reset() override;
bool canCompute() const override;
bool compute() override;
void setPose(const geometry_msgs::PoseStamped pose){
setProperty("pose", std::move(pose));
}
protected:
void onNewSolution(const SolutionBase& s) override;
std::deque<planning_scene::PlanningScenePtr> scenes_;
};
} } }

View File

@ -44,32 +44,34 @@ namespace moveit { namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(CartesianPath)
MOVEIT_CLASS_FORWARD(PipelinePlanner)
}
namespace stages {
/** Pick wraps a complete pipeline to pick up an object with a given end effector.
/** PickPlaceBase wraps the pipeline to pick or place an object with a given end effector.
*
* Picking consist of the following sub stages:
* - reaching to the object + "pre-grasp" end effector posture
* - linearly approaching the object along an approach direction/twist
* - "grasp" end effector posture
* - attach object
* - lift along along a given direction/twist
*
* Placing consist of the inverse order of stages:
* - place down along a given direction
* - detach the object
* - linearly retract end effector
*
* The end effector postures corresponding to pre-grasp and grasp as well as
* the end effector's Cartesian pose needs to be provided by an external grasp stage.
*/
class Pick : public SerialContainer {
class PickPlaceBase : public SerialContainer {
solvers::CartesianPathPtr cartesian_solver_;
solvers::PipelinePlannerPtr pipeline_solver_;
Stage* grasp_stage_ = nullptr;
Stage* approach_stage_ = nullptr;
Stage* lift_stage_ = nullptr;
public:
Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick");
PickPlaceBase(Stage::pointer &&grasp_stage, const std::string& name, bool forward);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
@ -81,12 +83,50 @@ public:
}
solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; }
solvers::PipelinePlannerPtr pipelineSolver() { return pipeline_solver_; }
void setApproachMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance);
void setLiftMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance);
void setApproachRetract(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance);
void setLiftPlace(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance);
};
/// specialization of PickPlaceBase to realize picking
class Pick : public PickPlaceBase {
public:
Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick")
: PickPlaceBase(std::move(grasp_stage), name, true)
{}
void setApproachMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
setApproachRetract(motion, min_distance, max_distance);
}
void setLiftMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance);
}
};
/// specialization of PickPlaceBase to realize placing
class Place : public PickPlaceBase {
public:
Place(Stage::pointer &&ungrasp_stage, const std::string& name = "place")
: PickPlaceBase(std::move(ungrasp_stage), name, false)
{}
void setRetractMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
setApproachRetract(motion, min_distance, max_distance);
}
void setPlaceMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance);
}
};
} } }

View File

@ -46,18 +46,20 @@ namespace moveit { namespace task_constructor { namespace stages {
class GenerateGraspPose;
/** Simple Grasp Stage
/** base class for simple grasping / ungrasping
*
* Given a named pre-grasp and grasp posture the stage generates
* fully-qualified pre-grasp and grasp robot states, connected
* by a grasping trajectory of the end-effector.
* Given a named pre-grasp and grasp posture the stage generates fully-qualified
* pre-grasp and grasp robot states, connected by a grasping trajectory of the end-effector.
*
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
*/
class SimpleGrasp : public SerialContainer {
class SimpleGraspBase : public SerialContainer {
moveit::core::RobotModelConstPtr model_;
GenerateGraspPose* grasp_generator_ = nullptr;
public:
SimpleGrasp(const std::string& name = "grasp");
SimpleGraspBase(const std::string& name, bool forward);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void setMonitoredStage(Stage* monitored);
@ -101,4 +103,18 @@ public:
}
};
/// specialization of SimpleGraspBase to realize grasping
class SimpleGrasp : public SimpleGraspBase {
public:
SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {}
};
/// specialization of SimpleGraspBase to realize ungrasping
class SimpleUnGrasp : public SimpleGraspBase {
public:
SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {}
};
} } }

View File

@ -172,11 +172,9 @@ private:
class StagePrivate;
class SubTrajectory;
/// SolutionTrajectory is composed of a series of SubTrajectories
typedef std::vector<const SubTrajectory*> SolutionTrajectory;
/// abstract base class for solutions (primitive and sequences)
class SolutionBase {
public:
inline const InterfaceState* start() const { return start_; }
@ -241,7 +239,7 @@ private:
};
// SubTrajectory connects interface states of ComputeStages
/// SubTrajectory connects interface states of ComputeStages
class SubTrajectory : public SolutionBase {
public:
SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr())
@ -260,6 +258,35 @@ private:
};
/** Sequence of individual sub solutions
*
* A solution sequence describes a solution that is composed from several individual
* sub solutions that need to be chained together to yield the overall solutions.
*/
class SolutionSequence : public SolutionBase {
public:
typedef std::vector<const SolutionBase*> container_type;
explicit SolutionSequence()
: SolutionBase()
{}
SolutionSequence(container_type&& subsolutions, double cost = 0.0, StagePrivate* creator = nullptr)
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
void push_back(const SolutionBase& solution);
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override;
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
private:
/// series of sub solutions
container_type subsolutions_;
};
template <> inline
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
return end_->outgoingTrajectories();

View File

@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/cost_queue.h
${PROJECT_INCLUDE}/introspection.h
${PROJECT_INCLUDE}/marker_tools.h
${PROJECT_INCLUDE}/merge.h
${PROJECT_INCLUDE}/properties.h
${PROJECT_INCLUDE}/stage.h
${PROJECT_INCLUDE}/stage_p.h
@ -18,6 +19,7 @@ add_library(${PROJECT_NAME}
container.cpp
introspection.cpp
marker_tools.cpp
merge.cpp
properties.cpp
stage.cpp
storage.cpp

View File

@ -283,18 +283,18 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
struct SolutionCollector {
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
void operator()(const SerialContainer::solution_container& trace, double cost) {
void operator()(const SolutionSequence::container_type& trace, double cost) {
// traced path should not extend past container boundaries
assert(trace.size() <= max_depth);
solutions.emplace_back(std::make_pair(trace, cost));
}
typedef std::list<std::pair<SerialContainer::solution_container, double>> SolutionCostPairs;
typedef std::list<std::pair<SolutionSequence::container_type, double>> SolutionCostPairs;
SolutionCostPairs solutions;
const size_t max_depth;
};
void updateStateCosts(const SerialContainer::solution_container &partial_solution_path,
void updateStateCosts(const SolutionSequence::container_type &partial_solution_path,
const InterfaceState::Priority &prio) {
for (const SolutionBase* solution : partial_solution_path) {
// here it suffices to update the start state, because the end state is the start state
@ -322,7 +322,7 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
assert(num_before < children.size()); // creator should be one of our children
num_after = children.size()-1 - num_before;
SerialContainer::solution_container trace; trace.reserve(children.size());
SolutionSequence::container_type trace; trace.reserve(children.size());
// find all incoming solution paths ending at current solution
SolutionCollector incoming(num_before);
@ -333,8 +333,8 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
// collect (and sort) all solutions spanning from start to end of this container
ordered<SerialSolution> sorted;
SerialContainer::solution_container solution;
ordered<SolutionSequence> sorted;
SolutionSequence::container_type solution;
solution.reserve(children.size());
for (auto& in : incoming.solutions) {
for (auto& out : outgoing.solutions) {
@ -352,7 +352,7 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
// insert outgoing solutions in normal order
solution.insert(solution.end(), out.first.begin(), out.first.end());
// store solution in sorted list
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
sorted.insert(SolutionSequence(std::move(solution), prio.cost(), impl));
} else if (prio.depth() > 1) {
// update state priorities along the whole partial solution path
updateStateCosts(in.first, prio);
@ -655,7 +655,7 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
template <Interface::Direction dir>
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
solution_container &trace, double trace_cost)
SolutionSequence::container_type &trace, double trace_cost)
{
const InterfaceState::Solutions& solutions = start.trajectories<dir>();
if (solutions.empty()) // if we reached the end, call the callback
@ -671,28 +671,6 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
}
}
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection) const
{
moveit_task_constructor_msgs::SubSolution sub_msg;
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
sub_msg.cost = this->cost();
const Introspection *ci = introspection;
sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
sub_msg.sub_solution_id.reserve(subsolutions_.size());
if (introspection) {
for (const SolutionBase* s : subsolutions_)
sub_msg.sub_solution_id.push_back(introspection->solutionId(*s));
msg.sub_solution.push_back(sub_msg);
}
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
for (const SolutionBase* s : subsolutions_)
s->fillMessage(msg, introspection);
}
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution,
Introspection *introspection) const
@ -878,7 +856,6 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
auto impl = pimpl();
assert(impl->prevEnds() && impl->nextStarts());
t.setCreator(impl);
// store newly created solution (otherwise it's lost)
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));

175
core/src/merge.cpp Normal file
View File

@ -0,0 +1,175 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Hamburg University
* 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: Luca Lach, Robert Haschke */
#include <moveit/task_constructor/merge.h>
namespace moveit { namespace task_constructor {
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups)
{
if (groups.size() <= 1)
throw std::runtime_error("Expected multiple groups");
const moveit::core::RobotModel* const robot_model = &groups[0]->getParentModel();
std::set<const moveit::core::JointModel*> jset;
std::vector<std::string> names;
for (const moveit::core::JointModelGroup* jmg : groups) {
// sanity check: all groups must share the same robot model
if (&jmg->getParentModel() != robot_model)
throw std::runtime_error("groups refer to different robot models");
const auto& joints = jmg->getJointModels();
jset.insert(joints.cbegin(), joints.cend());
names.push_back(jmg->getName());
}
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
// attention: do not use getConfig()
return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model);
}
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
std::vector<const moveit::core::JointModel*> joints,
std::vector<const moveit::core::JointModel*>& duplicates,
std::string& names)
{
duplicates.clear();
names.clear();
for (const moveit::core::JointModelGroup* jmg : groups) {
for (const moveit::core::JointModel* jm : jmg->getJointModels()) {
auto it = std::find(joints.begin(), joints.end(), jm);
if (it == joints.end()) { // jm not found anymore -> duplicate
if (jm->getType() != moveit::core::JointModel::FIXED && // fixed joints are OK
std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end()) {
duplicates.push_back(jm);
if (!names.empty()) names.append(", ");
names.append(jm->getName());
}
continue;
} else
joints.erase(it); // remove from list as processed
}
}
return duplicates.size() > 0;
}
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
moveit::core::JointModelGroup*& merged_group)
{
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
const std::vector<const moveit::core::JointModel*> *merged_joints
= merged_group ? &merged_group->getJointModels() : nullptr;
std::set<const moveit::core::JointModel*> jset;
std::vector<const moveit::core::JointModelGroup*> groups;
groups.reserve(sub_trajectories.size());
// sanity checks: all sub solutions must share the same robot model and use disjoint joint sets
const moveit::core::RobotModelConstPtr& robot_model = sub_trajectories[0]->getRobotModel();
size_t max_num_joints = 0; // maximum number of joints in sub groups
size_t num_joints = 0; // sum of joints in all sub groups
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
if (sub->getRobotModel() != robot_model)
throw std::runtime_error("subsolutions refer to multiple robot models");
const moveit::core::JointModelGroup* jmg = sub->getGroup();
groups.push_back(jmg);
const auto& joints = jmg->getJointModels();
if (merged_joints) { // validate that the joint model is known in merged_group
for (const moveit::core::JointModel* jm : joints) {
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
}
} else // accumulate set of joints
jset.insert(joints.cbegin(), joints.cend());
max_num_joints = std::max(max_num_joints, joints.size());
num_joints += joints.size();
}
size_t num_merged = merged_joints ? merged_joints->size() : jset.size();
if (num_merged != num_joints) {
// overlapping joint groups: analyse in more detail
std::vector<const moveit::core::JointModel*> joints;
if (merged_joints) joints = *merged_joints;
else joints.insert(joints.end(), jset.cbegin(), jset.cend());
std::vector<const moveit::core::JointModel*> duplicates;
std::string names;
if (findDuplicates(groups, joints, duplicates, names))
throw std::runtime_error("overlapping joint groups: " + names);
}
// create merged_group if necessary
if (!merged_group) {
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
merged_group = new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model.get());
}
// do the actual trajectory merging
auto merged_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, merged_group);
std::vector<double> values;
values.reserve(max_num_joints);
while (true) {
bool finished = true;
size_t index = merged_traj->getWayPointCount();
auto merged_state = index == 0 ? std::make_shared<robot_state::RobotState>(robot_model)
: std::make_shared<robot_state::RobotState>(merged_traj->getLastWayPoint());
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
if (index >= sub->getWayPointCount())
continue; // no more waypoints in this sub solution
finished = false; // there was a waypoint, continue while loop
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values);
}
if (finished)
break;
// add waypoint without timing
merged_state->update();
merged_traj->addSuffixWayPoint(merged_state, 0.0);
}
return merged_traj;
}
} }

View File

@ -38,11 +38,11 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/planning_scene/planning_scene.h>
#include <iostream>
#include <iomanip>
#include <ros/console.h>
using namespace std::placeholders;
namespace moveit { namespace task_constructor {
void InitStageException::push_back(const Stage &stage, const std::string &msg)
@ -83,8 +83,10 @@ InterfaceFlags StagePrivate::interfaceFlags() const
return f;
}
void StagePrivate::newSolution(const SolutionBase &solution)
void StagePrivate::newSolution(SolutionBase &solution)
{
solution.setCreator(this);
if (introspection_)
introspection_->registerSolution(solution);
@ -249,7 +251,6 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
}
SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) {
trajectory.setCreator(this);
if (!trajectory.isFailure()) {
return *solutions_.insert(std::move(trajectory));
} else if (me()->storeFailures()) {
@ -309,14 +310,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
{
if (dir & PropagatingEitherWay::FORWARD) {
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, _1)));
starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
} else {
starts_.reset();
}
if (dir & PropagatingEitherWay::BACKWARD) {
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, _1)));
ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
} else {
ends_.reset();
}
@ -572,7 +573,7 @@ void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_mod
if (!impl->monitored_)
throw InitStageException(*this, "no monitored stage defined");
if (!impl->registered_) { // register only once
impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGenerator::onNewSolution, this, _1));
impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGenerator::onNewSolution, this, std::placeholders::_1));
impl->registered_ = true;
}
}
@ -581,15 +582,27 @@ void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_mod
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: ComputeBasePrivate(me, name)
{
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2)));
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1, std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2)));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
return InterfaceFlags(CONNECT);
}
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
template <>
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::BACKWARD>
(Interface::const_iterator first, Interface::const_iterator second) {
return std::make_pair(first, second);
}
template <>
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>
(Interface::const_iterator first, Interface::const_iterator second) {
return std::make_pair(second, first);
}
template <Interface::Direction other>
void ConnectingPrivate::newState(Interface::iterator it, bool updated)
{
// TODO: only consider interface states with priority depth > threshold
if (!std::isfinite(it->priority().cost())) {
@ -602,30 +615,12 @@ void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
// many pairs might be affected: sort
pending.sort();
} else { // new state: insert all pairs with other interface
for (auto oit = ends_->begin(), oend = ends_->end(); oit != oend; ++oit) {
InterfacePtr other_interface = pullInterface(other);
for (auto oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!std::isfinite(oit->priority().cost()))
break;
pending.insert(std::make_pair(it, oit));
}
}
}
void ConnectingPrivate::newEndState(Interface::iterator it, bool updated)
{
if (!std::isfinite(it->priority().cost())) {
// remove pending pairs, if cost updated to infinity
if (updated)
pending.remove_if([it](const StatePair& p) { return p.second == it; });
return;
}
if (updated) {
// many pairs might be affected: sort
pending.sort();
} else { // new state: insert all pairs with other interface
for (auto oit = starts_->begin(), oend = starts_->end(); oit != oend; ++oit) {
if (!std::isfinite(oit->priority().cost()))
break;
pending.insert(std::make_pair(oit, it));
if (static_cast<Connecting*>(me_)->compatible(*it, *oit))
pending.insert(make_pair<other>(it, oit));
}
}
}
@ -653,13 +648,38 @@ void Connecting::reset()
ComputeBase::reset();
}
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory&& t) {
auto impl = pimpl();
SubTrajectory& trajectory = impl->addTrajectory(std::move(t));
trajectory.setStartState(from);
trajectory.setEndState(to);
impl->newSolution(trajectory);
/// compare consistency of planning scenes
bool Connecting::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const
{
const planning_scene::PlanningSceneConstPtr& from = from_state.scene();
const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
if (from->getWorld()->size() != to->getWorld()->size())
return false; // different number of collision objects
// both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) {
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
if (!to_object) return false; // object missing
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) return false; // shapes not matching
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it)
if (!from_it->matrix().array().isApprox(to_it->matrix().array()))
return false; // transforms do not match
}
return true;
}
void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) {
newSolution(from, to, pimpl()->addTrajectory(std::move(t)));
}
void Connecting::newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
solution.setStartState(from);
solution.setEndState(to);
pimpl()->newSolution(solution);
}
std::ostream& operator<<(std::ostream& os, const Stage& stage) {

View File

@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/current_state.h
${PROJECT_INCLUDE}/stages/fixed_state.h
${PROJECT_INCLUDE}/stages/generate_pose.h
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/compute_ik.h
@ -24,6 +25,7 @@ add_library(${PROJECT_NAME}_stages
current_state.cpp
fixed_state.cpp
generate_grasp_pose.cpp
generate_pose.cpp
compute_ik.cpp
connect.cpp

View File

@ -37,13 +37,14 @@
*/
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
Connect::Connect(std::string name, const GroupPlannerVector& planners)
: Connecting(name)
, planner_(planner)
, planner_(planners)
{
auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout");
@ -52,25 +53,203 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
"constraints to maintain during trajectory");
}
void Connect::reset()
{
Connecting::reset();
merged_jmg_.reset();
solutions_.clear();
subsolutions_.clear();
states_.clear();
}
void Connect::init(const core::RobotModelConstPtr& robot_model)
{
Connecting::init(robot_model);
planner_->init(robot_model);
InitStageException errors;
if (planner_.empty())
errors.push_back(*this, "empty set of groups");
size_t num_joints = 0;
std::vector<const moveit::core::JointModelGroup*> groups;
for (const GroupPlannerVector::value_type& pair : planner_) {
if (!robot_model->hasJointModelGroup(pair.first))
errors.push_back(*this, "invalid group: " + pair.first);
else if (!pair.second)
errors.push_back(*this, "invalid planner for group: " + pair.first);
else {
pair.second->init(robot_model);
auto jmg = robot_model->getJointModelGroup(pair.first);
groups.push_back(jmg);
num_joints += jmg->getJointModels().size();
}
}
if (!errors && groups.size() >= 2) { // enable merging?
merged_jmg_.reset(task_constructor::merge(groups));
if (merged_jmg_->getJointModels().size() != num_joints) {
// overlapping joint groups: analyse in more detail
std::vector<const moveit::core::JointModel*> duplicates;
std::string names;
if (findDuplicates(groups, merged_jmg_->getJointModels(), duplicates, names)) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": overlapping joint groups: " << names << ". Disabling merging.");
merged_jmg_.reset(); // fallback to serial connect
}
}
}
if (errors)
throw errors;
}
bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const
{
if (!Connecting::compatible(from_state, to_state))
return false;
const moveit::core::RobotState& from = from_state.scene()->getCurrentState();
const moveit::core::RobotState& to = to_state.scene()->getCurrentState();
// compose set of joint names we plan for
std::set<std::string> planned_joint_names;
for (const GroupPlannerVector::value_type& pair : planner_) {
const moveit::core::JointModelGroup *jmg = from.getJointModelGroup(pair.first);
const auto &names = jmg->getJointModelNames();
planned_joint_names.insert(names.begin(), names.end());
}
// all active joints that we don't plan for should match
for (const moveit::core::JointModel* jm : from.getRobotModel()->getJointModels()) {
if (planned_joint_names.count(jm->getName()))
continue; // ignore joints we plan for
const unsigned int num = jm->getVariableCount();
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
if (!positions_from.array().isApprox(positions_to.array())) {
ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName()
<< ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]");
return false;
}
}
return true;
}
bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
const auto& props = properties();
const std::string& group = props.get<std::string>("group");
double timeout = props.get<double>("timeout");
const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group);
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr trajectory;
if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory,
props.get<moveit_msgs::Constraints>("path_constraints")))
return false;
std::vector<robot_trajectory::RobotTrajectoryPtr> sub_trajectories;
planning_scene::PlanningScenePtr start = from.scene()->diff();
const moveit::core::RobotState& goal_state = to.scene()->getCurrentState();
connect(from, to, trajectory);
return true;
std::vector<planning_scene::PlanningScenePtr> intermediate_scenes;
intermediate_scenes.push_back(start);
std::vector<double> positions;
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
intermediate_scenes.push_back(end);
const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first);
goal_state.copyJointGroupPositions(jmg, positions);
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
robot_trajectory::RobotTrajectoryPtr trajectory;
if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints))
break;
sub_trajectories.push_back(trajectory);
// continue from reached state
start = end;
}
SolutionBase* solution = nullptr;
if (sub_trajectories.size() != planner_.size()) { // error during sequential planning
if (!storeFailures())
return false;
sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr());
solution = storeSequential(sub_trajectories, intermediate_scenes);
// mark solution as failure
solution->setCost(std::numeric_limits<double>::infinity());
} else {
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes);
if (t) {
connect(from, to, SubTrajectory(t));
return true;
}
// merging failed, store sequentially
solution = storeSequential(sub_trajectories, intermediate_scenes);
}
newSolution(from, to, *solution);
return !solution->isFailure();
}
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
{
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningScenePtr start = *scene_it;
SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) {
planning_scene::PlanningScenePtr end = *++scene_it;
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
inserted->setCreator(pimpl_);
// push back solution pointer
sub_solutions.push_back(&*inserted);
// provide meaningful start/end states
states_.emplace_back(InterfaceState(start));
subsolutions_.back().setStartState(states_.back());
states_.emplace_back(InterfaceState(end));
subsolutions_.back().setEndState(states_.back());
start = end;
}
solutions_.emplace_back(SolutionSequence(std::move(sub_solutions)));
return &solutions_.back();
}
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
{
auto jmg = merged_jmg_.get();
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, jmg);
return trajectory;
}
size_t Connect::numSolutions() const
{
return solutions_.size() + Connecting::numSolutions();
}
void Connect::processSolutions(const Stage::SolutionProcessor& processor) const
{
// TODO: This is not nice, but necessary to process simple SubTrajectory + SolutionSequence
for (const auto& s : solutions_) {
if (s.isFailure()) continue;
if (!processor(s))
break;
}
Connecting::processSolutions(processor);
}
void Connect::processFailures(const Stage::SolutionProcessor &processor) const
{
// TODO: This is not nice, but necessary to process simple SubTrajectory + SolutionSequence
for (const auto& s : solutions_) {
if (!s.isFailure()) continue;
if (!processor(s))
break;
}
Connecting::processFailures(processor);
}
} } }

View File

@ -47,7 +47,7 @@
namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(const std::string& name)
: MonitoringGenerator(name)
: GeneratePose(name)
{
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector");
@ -56,15 +56,15 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name)
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
}
void GenerateGraspPose::setEndEffector(const std::string &eef){
void GenerateGraspPose::setEndEffector(const std::string &eef) {
setProperty("eef", eef);
}
void GenerateGraspPose::setNamedPose(const std::string &pose_name){
void GenerateGraspPose::setNamedPose(const std::string &pose_name) {
setProperty("pregrasp", pose_name);
}
void GenerateGraspPose::setObject(const std::string &object){
void GenerateGraspPose::setObject(const std::string &object) {
setProperty("object", object);
}
@ -72,50 +72,73 @@ void GenerateGraspPose::setAngleDelta(double delta){
setProperty("angle_delta", delta);
}
void GenerateGraspPose::reset()
void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
{
scene_.reset();
current_angle_ = 0.0;
MonitoringGenerator::reset();
InitStageException errors;
try { GeneratePose::init(robot_model); }
catch (InitStageException &e) { errors.append(e); }
const auto& props = properties();
// check angle_delta
if (props.get<double>("angle_delta") == 0.)
errors.push_back(*this, "angle_delta must be non-zero");
// check availability of eef
const std::string& eef = props.get<std::string>("eef");
if (!robot_model->hasEndEffector(eef))
errors.push_back(*this, "unknown end effector: " + eef);
else {
// check availability of eef pose
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
const std::string& name = props.get<std::string>("pregrasp");
std::map<std::string, double> m;
if (!jmg->getVariableDefaultPositions(name, m))
errors.push_back(*this, "unknown end effector pose: " + name);
}
if (errors) throw errors;
}
void GenerateGraspPose::onNewSolution(const SolutionBase& s)
{
if (scene_)
ROS_WARN_NAMED("GenerateGraspPose", "got additional solution from monitored stage");
scene_ = s.end()->scene()->diff();
}
planning_scene::PlanningScenePtr scene = s.end()->scene()->diff();
bool GenerateGraspPose::canCompute() const{
return scene_ && current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
}
bool GenerateGraspPose::compute(){
// set end effector pose
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
assert(scene_->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf");
const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef);
robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst();
const std::string& joint_pose = props.get<std::string>("pregrasp");
if(!joint_pose.empty()){
robot_state.setToDefaultValues(jmg , joint_pose);
}
robot_state::RobotState &robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg , props.get<std::string>("pregrasp"));
const std::string& object_name = props.get<std::string>("object");
if (!scene_->knowsFrameTransform(object_name))
throw std::runtime_error("requested object does not exist or could not be retrieved");
if (!scene->knowsFrameTransform(object_name)) {
ROS_WARN_STREAM_NAMED("GenerateGraspPose", "unknown object: " << object_name);
return;
}
scenes_.push_back(scene);
}
bool GenerateGraspPose::compute() {
if (scenes_.empty())
return false;
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
scenes_.pop_front();
const auto& props = properties();
geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = object_name;
target_pose_msg.header.frame_id = props.get<std::string>("object");
while( canCompute() ) {
double current_angle_ = 0.0;
while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) {
// rotate object pose about z-axis
Eigen::Affine3d target_pose(Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()));
current_angle_ += props.get<double>("angle_delta");
InterfaceState state(scene_);
InterfaceState state(scene);
tf::poseEigenToMsg(target_pose, target_pose_msg.pose);
state.properties().set("target_pose", target_pose_msg);
@ -127,10 +150,8 @@ bool GenerateGraspPose::compute(){
rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "grasp frame");
spawn(std::move(state), std::move(trajectory));
return true;
}
return false;
return true;
}
} } }

View File

@ -0,0 +1,93 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Bielefeld + Hamburg 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, Michael Goerner */
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
namespace moveit { namespace task_constructor { namespace stages {
GeneratePose::GeneratePose(const std::string& name)
: MonitoringGenerator(name)
{
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");
}
void GeneratePose::reset()
{
MonitoringGenerator::reset();
scenes_.clear();
}
void GeneratePose::onNewSolution(const SolutionBase& s)
{
scenes_.push_back(s.end()->scene()->diff());
}
bool GeneratePose::canCompute() const {
return scenes_.size() > 0;
}
bool GeneratePose::compute() {
if (scenes_.empty())
return false;
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
scenes_.pop_front();
geometry_msgs::PoseStamped target_pose = properties().get<geometry_msgs::PoseStamped>("pose");
if (target_pose.header.frame_id.empty())
target_pose.header.frame_id = scene->getPlanningFrame();
else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
return false;
}
InterfaceState state(scene);
state.properties().set("target_pose", target_pose);
SubTrajectory trajectory;
trajectory.setCost(0.0);
rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "pose frame");
spawn(std::move(state), std::move(trajectory));
return true;
}
} } }

View File

@ -1,19 +1,15 @@
#include <moveit/task_constructor/stages/pick.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward)
: SerialContainer(name)
{
PropertyMap& p = properties();
@ -26,51 +22,34 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
pipeline_solver_ = std::make_shared<solvers::PipelinePlanner>();
pipeline_solver_->setTimeout(8.0);
pipeline_solver_->setPlannerId("RRTConnectkConfigDefault");
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
{
auto move = std::make_unique<MoveTo>("open gripper", pipeline_solver_);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_group");
move->setGoal("open"); // TODO: retrieve from grasp stage
insert(std::move(move));
}
{
auto move = std::make_unique<Connect>("move to object", pipeline_solver_);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
insert(std::move(move));
}
{
auto approach = std::make_unique<MoveRelative>("approach object", cartesian_solver_);
auto approach = std::make_unique<MoveRelative>(forward ? "approach object" : "retract", cartesian_solver_);
PropertyMap& p = approach->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.property("link").configureInitFrom(Stage::PARENT, "eef_frame");
p.set("marker_ns", std::string("approach"));
p.set("marker_ns", std::string(forward ? "approach" : "retract"));
approach_stage_ = approach.get();
insert(std::move(approach));
insert(std::move(approach), insertion_position);
}
grasp_stage_ = grasp_stage.get();
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
insert(std::move(grasp_stage));
insert(std::move(grasp_stage), insertion_position);
{
auto lift = std::make_unique<MoveRelative>("lift object", cartesian_solver_);
auto lift = std::make_unique<MoveRelative>(forward ? "lift object" : "place object", cartesian_solver_);
PropertyMap& p = lift->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.property("link").configureInitFrom(Stage::PARENT, "eef_frame");
p.set("marker_ns", std::string("lift"));
p.set("marker_ns", std::string(forward ? "lift" : "place"));
lift_stage_ = lift.get();
insert(std::move(lift));
insert(std::move(lift), insertion_position);
}
}
void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{
// inherit properties from parent
PropertyMap* p = &properties();
@ -79,6 +58,8 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
// init internal properties
const std::string &eef = p->get<std::string>("eef");
const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef);
if (!jmg) throw InitStageException(*this, "unknown end effector: " + eef);
p->set<std::string>("eef_group", jmg->getName());
p->set<std::string>("eef_parent_group", jmg->getEndEffectorParentGroup().first);
@ -86,7 +67,7 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
SerialContainer::init(robot_model);
}
void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
{
auto& p = approach_stage_->properties();
p.set("twist", motion);
@ -94,7 +75,7 @@ void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double m
p.set("max_distance", max_distance);
}
void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
{
auto& p = lift_stage_->properties();
p.set("twist", motion);

View File

@ -49,23 +49,24 @@
namespace moveit { namespace task_constructor { namespace stages {
SimpleGrasp::SimpleGrasp(const std::string& name)
SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
: SerialContainer(name)
{
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
{
auto gengrasp = std::make_unique<GenerateGraspPose>("generate grasp pose");
auto gengrasp = std::make_unique<GenerateGraspPose>(forward ? "generate grasp pose" : "generate release pose");
grasp_generator_ = gengrasp.get();
auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp));
const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" };
ik->exposePropertiesOfChild(0, grasp_prop_names);
insert(std::move(ik));
insert(std::move(ik), insertion_position);
exposePropertiesOfChild(-1, grasp_prop_names);
exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout", "ik_frame" });
exposePropertiesOfChild(insertion_position, grasp_prop_names);
exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" });
}
{
auto allow_touch = std::make_unique<ModifyPlanningScene>("allow object collision");
auto allow_touch = std::make_unique<ModifyPlanningScene>(forward ? "allow object collision" : "forbid object collision");
PropertyMap& p = allow_touch->properties();
p.declare<std::string>("eef");
p.declare<std::string>("object");
@ -78,53 +79,54 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
->getLinkModelNamesWithCollisionGeometry(), true);
});
insert(std::move(allow_touch));
insert(std::move(allow_touch), insertion_position);
}
{
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
auto move = std::make_unique<MoveTo>(forward ? "close gripper" : "open gripper", pipeline);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
const std::string& eef = parent_map.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
return boost::any(jmg->getName());
});
insert(std::move(move));
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
insert(std::move(move), insertion_position);
exposePropertyOfChildAs(insertion_position, "joint_pose", forward ? "grasp" : "pregrasp");
}
{
auto attach = std::make_unique<ModifyPlanningScene>("attach object");
auto attach = std::make_unique<ModifyPlanningScene>(forward ? "attach object" : "detach object");
PropertyMap& p = attach->properties();
p.declare<std::string>("eef");
p.declare<std::string>("object");
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
attach->setCallback([this, forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
const std::string& eef = p.get<std::string>("eef");
moveit_msgs::AttachedCollisionObject obj;
obj.object.operation = moveit_msgs::CollisionObject::ADD;
obj.object.operation = forward ? (int8_t) moveit_msgs::CollisionObject::ADD
: (int8_t) moveit_msgs::CollisionObject::REMOVE;
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
obj.object.id = p.get<std::string>("object");
scene->processAttachedCollisionObjectMsg(obj);
});
insert(std::move(attach));
insert(std::move(attach), insertion_position);
}
}
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{
model_ = robot_model;
SerialContainer::init(robot_model);
}
void SimpleGrasp::setMonitoredStage(Stage* monitored)
void SimpleGraspBase::setMonitoredStage(Stage* monitored)
{
grasp_generator_->setMonitoredStage(monitored);
}
void SimpleGrasp::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);

View File

@ -127,7 +127,6 @@ void SolutionBase::setCost(double cost) {
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection *introspection) const {
msg.sub_trajectory.emplace_back();
@ -150,4 +149,33 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
}
void SolutionSequence::push_back(const SolutionBase& solution)
{
subsolutions_.push_back(&solution);
}
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection) const
{
moveit_task_constructor_msgs::SubSolution sub_msg;
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
sub_msg.cost = this->cost();
const Introspection *ci = introspection;
sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
sub_msg.sub_solution_id.reserve(subsolutions_.size());
if (introspection) {
for (const SolutionBase* s : subsolutions_)
sub_msg.sub_solution_id.push_back(introspection->solutionId(*s));
msg.sub_solution.push_back(sub_msg);
}
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
for (const SolutionBase* s : subsolutions_)
s->fillMessage(msg, introspection);
}
} }