mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'master' 'generate-pose' and 'connect'
This commit is contained in:
commit
70065c98e7
@ -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));
|
||||
}
|
||||
|
||||
@ -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"));
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
69
core/include/moveit/task_constructor/merge.h
Normal file
69
core/include/moveit/task_constructor/merge.h
Normal 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);
|
||||
|
||||
} }
|
||||
@ -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);
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -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 ¤t);
|
||||
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;
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
64
core/include/moveit/task_constructor/stages/generate_pose.h
Normal file
64
core/include/moveit/task_constructor/stages/generate_pose.h
Normal 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_;
|
||||
};
|
||||
|
||||
} } }
|
||||
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
} } }
|
||||
|
||||
@ -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) {}
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 ¤t)
|
||||
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 ¤t)
|
||||
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 ¤t)
|
||||
// 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
175
core/src/merge.cpp
Normal 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;
|
||||
}
|
||||
|
||||
} }
|
||||
@ -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) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
93
core/src/stages/generate_pose.cpp
Normal file
93
core/src/stages/generate_pose.cpp
Normal 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;
|
||||
}
|
||||
|
||||
} } }
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
} }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user