mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'joint_pose', 'move-relative-joints', 'basic-merger' and 'compute-ik'
This commit is contained in:
commit
495c80350b
@ -132,9 +132,9 @@ class ParallelContainerBasePrivate;
|
||||
class ParallelContainerBase;
|
||||
/** Parallel containers allow for alternative planning stages
|
||||
* Parallel containers can come in different flavours:
|
||||
* - alternatives: each child stage can contribute a solution
|
||||
* - fallbacks: the children are considered in series
|
||||
* - merged: solutions of all children (actuating disjoint groups)
|
||||
* - Alternatives: each child stage can contribute a solution
|
||||
* - Fallbacks: the children are considered in series
|
||||
* - Merger: solutions of all children (actuating disjoint groups)
|
||||
* are merged into a single solution for parallel execution
|
||||
*/
|
||||
class ParallelContainerBase : public ContainerBase
|
||||
@ -175,6 +175,10 @@ protected:
|
||||
trajectory.setCost(cost);
|
||||
spawn(std::move(state), std::move(trajectory));
|
||||
}
|
||||
/// propagate a solution forwards
|
||||
void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
|
||||
/// propagate a solution backwards
|
||||
void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
|
||||
};
|
||||
|
||||
|
||||
@ -212,6 +216,25 @@ public:
|
||||
};
|
||||
|
||||
|
||||
class MergerPrivate;
|
||||
/** Plan for different sub tasks in parallel and finally merge all sub solutions into a single trajectory */
|
||||
class Merger : public ParallelContainerBase
|
||||
{
|
||||
public:
|
||||
PRIVATE_CLASS(Merger)
|
||||
Merger(const std::string &name = "merger");
|
||||
|
||||
void reset() override;
|
||||
void init(const core::RobotModelConstPtr &robot_model) override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
protected:
|
||||
Merger(MergerPrivate* impl);
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
};
|
||||
|
||||
|
||||
class WrapperBasePrivate;
|
||||
/** A wrapper wraps a single child stage, which can be accessed via wrapped().
|
||||
*
|
||||
|
||||
@ -39,11 +39,17 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include "stage_p.h"
|
||||
|
||||
#include <map>
|
||||
#include <climits>
|
||||
|
||||
namespace moveit { namespace core {
|
||||
MOVEIT_CLASS_FORWARD(JointModelGroup)
|
||||
MOVEIT_CLASS_FORWARD(RobotState)
|
||||
} }
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
/* A container needs to decouple its own (external) interfaces
|
||||
@ -121,7 +127,10 @@ protected:
|
||||
void liftSolution(SolutionBase& solution,
|
||||
const InterfaceState *internal_from, const InterfaceState *internal_to);
|
||||
|
||||
protected:
|
||||
auto& internalToExternalMap() { return internal_to_external_; }
|
||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||
|
||||
private:
|
||||
container_type children_;
|
||||
|
||||
// map start/end states of children (internal) to corresponding states in our external interfaces
|
||||
@ -205,6 +214,17 @@ public:
|
||||
// called by parent asking for pruning of this' interface
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
|
||||
// grant access to protected methods in ParallelContainerBase
|
||||
inline void spawn(InterfaceState &&state, SubTrajectory&& t) {
|
||||
static_cast<ParallelContainerBase*>(me_)->spawn(std::move(state), std::move(t));
|
||||
}
|
||||
inline void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
|
||||
static_cast<ParallelContainerBase*>(me_)->sendForward(from, std::move(to), std::move(t));
|
||||
}
|
||||
inline void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
|
||||
static_cast<ParallelContainerBase*>(me_)->sendBackward(std::move(from), to, std::move(t));
|
||||
}
|
||||
|
||||
private:
|
||||
/// callback for new externally received states
|
||||
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
||||
@ -232,4 +252,32 @@ public:
|
||||
};
|
||||
PIMPL_FUNCTIONS(WrapperBase)
|
||||
|
||||
|
||||
class MergerPrivate : public ParallelContainerBasePrivate {
|
||||
friend class Merger;
|
||||
|
||||
moveit::core::JointModelGroupPtr jmg_merged_;
|
||||
typedef std::vector<const SubTrajectory*> ChildSolutionList;
|
||||
typedef std::map<const StagePrivate*, ChildSolutionList> ChildSolutionMap;
|
||||
// map from external source state (iterator) to all corresponding children's solutions
|
||||
std::map<InterfaceState*, ChildSolutionMap> source_state_to_solutions_;
|
||||
|
||||
public:
|
||||
typedef std::function<void(SubTrajectory&&)> Spawner;
|
||||
MergerPrivate(Merger* me, const std::string& name);
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
|
||||
void onNewPropagateSolution(const SolutionBase& s);
|
||||
void onNewGeneratorSolution(const SolutionBase& s);
|
||||
void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
|
||||
const moveit::core::RobotState& state, const Spawner& spawner);
|
||||
void merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner);
|
||||
|
||||
void sendForward(SubTrajectory&& t, const InterfaceState* from);
|
||||
void sendBackward(SubTrajectory&& t, const InterfaceState* to);
|
||||
void spawn(SubTrajectory&& t);
|
||||
};
|
||||
PIMPL_FUNCTIONS(Merger)
|
||||
|
||||
} }
|
||||
|
||||
@ -54,9 +54,13 @@ namespace moveit { namespace task_constructor { namespace stages {
|
||||
* The wrapper reads a target_pose from the interface state of solutions provided
|
||||
* by the wrapped stage. This Cartesian pose (PoseStamped msg) is used as a goal
|
||||
* pose for inverse kinematics.
|
||||
*
|
||||
* Usually, the end effector's parent link or the group's tip link is used as
|
||||
* the IK frame, which should be moved to the goal frame. However, any other
|
||||
* IK frame can be defined (which is linked to the tip of the group).
|
||||
*
|
||||
* Properties of the internally received InterfaceState can be forwarded to the
|
||||
* newly generated, externally exposed InterfaceState.
|
||||
*/
|
||||
class ComputeIK : public WrapperBase {
|
||||
public:
|
||||
@ -65,11 +69,17 @@ public:
|
||||
void init(const core::RobotModelConstPtr &robot_model);
|
||||
void onNewSolution(const SolutionBase &s) override;
|
||||
|
||||
void setTimeout(double timeout);
|
||||
void setEndEffector(const std::string& eef);
|
||||
void setTimeout(double timeout) {
|
||||
setProperty("timeout", timeout);
|
||||
}
|
||||
void setEndEffector(const std::string& eef) {
|
||||
setProperty("eef", eef);
|
||||
}
|
||||
|
||||
/// setters for IK frame
|
||||
void setIKFrame(const geometry_msgs::PoseStamped &pose);
|
||||
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
|
||||
setProperty("ik_frame", pose);
|
||||
}
|
||||
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
|
||||
template <typename T>
|
||||
void setIKFrame(const T& p, const std::string& link) {
|
||||
@ -81,7 +91,9 @@ public:
|
||||
}
|
||||
|
||||
/// setters for target pose
|
||||
void setTargetPose(const geometry_msgs::PoseStamped &pose);
|
||||
void setTargetPose(const geometry_msgs::PoseStamped &pose) {
|
||||
setProperty("target_pose", pose);
|
||||
}
|
||||
void setTargetPose(const Eigen::Affine3d& pose, const std::string& frame = "");
|
||||
template <typename T>
|
||||
void setTargetPose(const T& p, const std::string& frame = "") {
|
||||
@ -89,8 +101,16 @@ public:
|
||||
setTargetPose(pose, frame);
|
||||
}
|
||||
|
||||
void setMaxIKSolutions(uint32_t n);
|
||||
void setIgnoreCollisions(bool flag);
|
||||
void setMaxIKSolutions(uint32_t n) {
|
||||
setProperty("max_ik_solutions", n);
|
||||
}
|
||||
void setIgnoreCollisions(bool flag) {
|
||||
setProperty("ignore_collisions", flag);
|
||||
}
|
||||
|
||||
void setForwardedProperties(const std::set<std::string>& names) {
|
||||
setProperty("forward_properties", names);
|
||||
}
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -55,22 +55,42 @@ public:
|
||||
bool computeForward(const InterfaceState& from) override;
|
||||
bool computeBackward(const InterfaceState& to) override;
|
||||
|
||||
void setGroup(const std::string& group);
|
||||
void setLink(const std::string& link);
|
||||
void setGroup(const std::string& group) {
|
||||
setProperty("group", group);
|
||||
}
|
||||
void setLink(const std::string& link) {
|
||||
setProperty("link", link);
|
||||
}
|
||||
|
||||
/// set minimum / maximum distance to move
|
||||
void setMinDistance(double distance);
|
||||
void setMaxDistance(double distance);
|
||||
void setMinMaxDistance(double min_distance, double max_distance);
|
||||
void setMinDistance(double distance) {
|
||||
setProperty("min_distance", distance);
|
||||
}
|
||||
void setMaxDistance(double distance) {
|
||||
setProperty("max_distance", distance);
|
||||
}
|
||||
void setMinMaxDistance(double min_distance, double max_distance) {
|
||||
setProperty("min_distance", min_distance);
|
||||
setProperty("max_distance", max_distance);
|
||||
}
|
||||
|
||||
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
||||
void setPathConstraints(moveit_msgs::Constraints path_constraints) {
|
||||
setProperty("path_constraints", std::move(path_constraints));
|
||||
}
|
||||
|
||||
/// perform twist motion on specified link
|
||||
void along(const geometry_msgs::TwistStamped& twist);
|
||||
void along(const geometry_msgs::TwistStamped& twist) {
|
||||
setProperty("twist", twist);
|
||||
}
|
||||
/// translate link along given direction
|
||||
void along(const geometry_msgs::Vector3Stamped& direction);
|
||||
void along(const geometry_msgs::Vector3Stamped& direction) {
|
||||
setProperty("direction", direction);
|
||||
}
|
||||
/// move specified joint variables by given amount
|
||||
void about(const std::map<std::string, double>& goal) {
|
||||
setProperty("joints", goal);
|
||||
}
|
||||
|
||||
|
||||
protected:
|
||||
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||
|
||||
@ -86,8 +86,10 @@ public:
|
||||
|
||||
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);
|
||||
void setLiftPlace(const std::map<std::string, double>& joints);
|
||||
};
|
||||
|
||||
|
||||
@ -107,6 +109,9 @@ public:
|
||||
double min_distance, double max_distance) {
|
||||
setLiftPlace(motion, min_distance, max_distance);
|
||||
}
|
||||
void setLiftMotion(const std::map<std::string, double>& joints) {
|
||||
setLiftPlace(joints);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -126,6 +131,9 @@ public:
|
||||
double min_distance, double max_distance) {
|
||||
setLiftPlace(motion, min_distance, max_distance);
|
||||
}
|
||||
void setPlaceMotion(const std::map<std::string, double>& joints) {
|
||||
setLiftPlace(joints);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -35,8 +35,9 @@
|
||||
/* Authors: Robert Haschke */
|
||||
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/merge.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <memory>
|
||||
@ -479,9 +480,13 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
|
||||
// initialize this' pull interfaces if first/last child pulls
|
||||
if (const InterfacePtr& target = (*start)->pimpl()->starts())
|
||||
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
|
||||
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl,
|
||||
std::placeholders::_1, std::cref(target),
|
||||
std::placeholders::_2)));
|
||||
if (const InterfacePtr& target = (*last)->pimpl()->ends())
|
||||
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
|
||||
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl,
|
||||
std::placeholders::_1, std::cref(target),
|
||||
std::placeholders::_2)));
|
||||
}
|
||||
|
||||
// called by parent asking for pruning of this' interface
|
||||
@ -767,11 +772,11 @@ void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_m
|
||||
// initialize this' pull connections
|
||||
impl->starts().reset(required & READS_START
|
||||
? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState,
|
||||
impl, Interface::FORWARD, _1, _2))
|
||||
impl, Interface::FORWARD, std::placeholders::_1, std::placeholders::_2))
|
||||
: nullptr);
|
||||
impl->ends().reset(required & READS_END
|
||||
? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState,
|
||||
impl, Interface::BACKWARD, _1, _2))
|
||||
impl, Interface::BACKWARD, std::placeholders::_1, std::placeholders::_2))
|
||||
: nullptr);
|
||||
|
||||
// initialize push connections of children according to their demands
|
||||
@ -875,6 +880,46 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
|
||||
impl->newSolution(*it);
|
||||
}
|
||||
|
||||
void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
assert(impl->nextStarts());
|
||||
|
||||
// store newly created solution (otherwise it's lost)
|
||||
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
|
||||
it->setStartState(from);
|
||||
|
||||
if (it->isFailure()) {
|
||||
auto state_it = impl->states_.insert(impl->states_.end(), std::move(to));
|
||||
it->setEndState(*state_it);
|
||||
impl->failures_.push_back(&*it);
|
||||
} else {
|
||||
impl->nextStarts()->add(std::move(to), &*it, NULL);
|
||||
impl->solutions_.insert(&*it);
|
||||
}
|
||||
impl->newSolution(*it);
|
||||
}
|
||||
|
||||
void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
assert(impl->prevEnds());
|
||||
|
||||
// store newly created solution (otherwise it's lost)
|
||||
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
|
||||
it->setEndState(to);
|
||||
|
||||
if (it->isFailure()) {
|
||||
auto state_it = impl->states_.insert(impl->states_.end(), std::move(from));
|
||||
it->setStartState(*state_it);
|
||||
impl->failures_.push_back(&*it);
|
||||
} else {
|
||||
impl->prevEnds()->add(std::move(from), NULL, &*it);
|
||||
impl->solutions_.insert(&*it);
|
||||
}
|
||||
impl->newSolution(*it);
|
||||
}
|
||||
|
||||
|
||||
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
|
||||
: ParallelContainerBasePrivate(me, name)
|
||||
@ -982,4 +1027,226 @@ bool Fallbacks::compute()
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
MergerPrivate::MergerPrivate(Merger *me, const std::string &name)
|
||||
: ParallelContainerBasePrivate(me, name)
|
||||
{}
|
||||
|
||||
InterfaceFlags MergerPrivate::requiredInterface() const
|
||||
{
|
||||
if (children().size() < 2)
|
||||
throw InitStageException(*me_, "Need 2 children at least.");
|
||||
|
||||
InterfaceFlags required = ParallelContainerBasePrivate::requiredInterface();
|
||||
|
||||
// all children need to share a common interface
|
||||
for (const Stage::pointer& stage : children()) {
|
||||
InterfaceFlags current = stage->pimpl()->requiredInterface();
|
||||
if (current != required)
|
||||
throw InitStageException(*stage, "Interface doesn't match the common one.");
|
||||
}
|
||||
|
||||
switch (required) {
|
||||
case PROPAGATE_FORWARDS:
|
||||
case PROPAGATE_BACKWARDS:
|
||||
case UNKNOWN:
|
||||
break; // these are supported
|
||||
case GENERATE:
|
||||
throw InitStageException(*me_, "Generator stages not yet supported.");
|
||||
case CONNECT:
|
||||
throw InitStageException(*me_, "Cannot merge connecting stages. Use Connect.");
|
||||
default:
|
||||
throw InitStageException(*me_, "Children's interface not supported.");
|
||||
}
|
||||
return required;
|
||||
}
|
||||
|
||||
|
||||
Merger::Merger(const std::string &name)
|
||||
: Merger(new MergerPrivate(this, name))
|
||||
{}
|
||||
|
||||
void Merger::reset()
|
||||
{
|
||||
ParallelContainerBase::reset();
|
||||
auto impl = pimpl();
|
||||
impl->jmg_merged_.reset();
|
||||
impl->source_state_to_solutions_.clear();
|
||||
}
|
||||
|
||||
void Merger::init(const core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
ParallelContainerBase::init(robot_model);
|
||||
}
|
||||
|
||||
Merger::Merger(MergerPrivate *impl)
|
||||
: ParallelContainerBase(impl)
|
||||
{
|
||||
}
|
||||
|
||||
bool Merger::canCompute() const
|
||||
{
|
||||
for (const auto& stage : pimpl()->children())
|
||||
if (stage->pimpl()->canCompute())
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Merger::compute()
|
||||
{
|
||||
bool success = false;
|
||||
for (const auto& stage : pimpl()->children()) {
|
||||
try {
|
||||
success |= stage->pimpl()->compute();
|
||||
} catch (const Property::error &e) {
|
||||
stage->reportPropertyError(e);
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
void Merger::onNewSolution(const SolutionBase& s)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
switch (impl->interfaceFlags()) {
|
||||
case PROPAGATE_FORWARDS:
|
||||
case PROPAGATE_BACKWARDS:
|
||||
impl->onNewPropagateSolution(s);
|
||||
break;
|
||||
case GENERATE:
|
||||
impl->onNewGeneratorSolution(s);
|
||||
break;
|
||||
default:
|
||||
assert(false);
|
||||
}
|
||||
}
|
||||
|
||||
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s)
|
||||
{
|
||||
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
|
||||
if (!trajectory) {
|
||||
ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported");
|
||||
return;
|
||||
}
|
||||
|
||||
InterfaceFlags dir = interfaceFlags();
|
||||
assert(dir == PROPAGATE_FORWARDS || dir == PROPAGATE_BACKWARDS);
|
||||
// internal source state
|
||||
const InterfaceState* source_state = (dir == PROPAGATE_FORWARDS) ? s.start() : s.end();
|
||||
|
||||
// map to external source state that is shared by all children
|
||||
auto source_it = internalToExternalMap().find(source_state);
|
||||
// internal->external mapping for source state should have been created
|
||||
assert(source_it != internalToExternalMap().end());
|
||||
InterfaceState* external_source_state = &*source_it->second;
|
||||
|
||||
// retrieve (or create if necessary) the ChildSolutionMap for the given external source state
|
||||
ChildSolutionMap& all_solutions = source_state_to_solutions_.insert(std::make_pair(external_source_state, ChildSolutionMap())).first->second;
|
||||
|
||||
// retrieve (or create if necessary) the ChildSolutionList corresponding to the child
|
||||
ChildSolutionList& child_solutions = all_solutions.insert(std::make_pair(s.creator(), ChildSolutionList())).first->second;
|
||||
// insert the new child solution into the list
|
||||
child_solutions.push_back(trajectory);
|
||||
|
||||
// do we have solutions for all children?
|
||||
if (all_solutions.size() < children().size()) return;
|
||||
assert(all_solutions.size() == children().size());
|
||||
|
||||
// combine the new solution with all solutions from other children
|
||||
auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward;
|
||||
mergeAnyCombination(all_solutions, s, external_source_state->scene()->getCurrentState(),
|
||||
std::bind(spawner, this, std::placeholders::_1, external_source_state));
|
||||
}
|
||||
|
||||
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from)
|
||||
{
|
||||
// generate target state
|
||||
planning_scene::PlanningScenePtr to = from->scene()->diff();
|
||||
to->setCurrentState(t.trajectory()->getLastWayPoint());
|
||||
ParallelContainerBasePrivate::sendForward(*from, InterfaceState(to), std::move(t));
|
||||
}
|
||||
|
||||
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to)
|
||||
{
|
||||
// generate target state
|
||||
planning_scene::PlanningScenePtr from = to->scene()->diff();
|
||||
from->setCurrentState(t.trajectory()->getFirstWayPoint());
|
||||
ParallelContainerBasePrivate::sendBackward(InterfaceState(from), *to, std::move(t));
|
||||
}
|
||||
|
||||
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s)
|
||||
{
|
||||
// TODO
|
||||
}
|
||||
|
||||
void MergerPrivate::spawn(SubTrajectory&& t)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
|
||||
const moveit::core::RobotState& state, const Spawner& spawner)
|
||||
{
|
||||
std::vector<size_t> indeces; // which solution index was considered last for i-th child?
|
||||
indeces.reserve(children().size());
|
||||
|
||||
ChildSolutionList sub_solutions;
|
||||
sub_solutions.reserve(children().size());
|
||||
|
||||
// initialize vector of sub solutions
|
||||
for (const auto &pair : all_solutions) {
|
||||
// all children, except current solution's creator, start with zero index
|
||||
indeces.push_back(pair.first != current.creator() ? 0 : pair.second.size()-1);
|
||||
sub_solutions.push_back(pair.second[indeces.back()]);
|
||||
}
|
||||
while (true) {
|
||||
merge(sub_solutions, state, spawner);
|
||||
|
||||
// compose next combination
|
||||
size_t child = 0;
|
||||
for (auto it = all_solutions.cbegin(), end = all_solutions.cend(); it != end; ++it, ++child) {
|
||||
if (it->first == current.creator()) continue; // skip current solution's child
|
||||
if (++indeces[child] >= it->second.size()) {
|
||||
indeces[child] = 0; // start over with zero
|
||||
sub_solutions[child] = it->second[indeces[child]];
|
||||
continue; // and continue with next child
|
||||
}
|
||||
// otherwise, a new solution combination is available
|
||||
sub_solutions[child] = it->second[indeces[child]];
|
||||
break;
|
||||
}
|
||||
if (child == children().size()) // all combinations exhausted?
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner)
|
||||
{
|
||||
// transform vector of SubTrajectories into vector of RobotTrajectories
|
||||
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
|
||||
sub_trajectories.reserve(sub_solutions.size());
|
||||
for (const auto& sub : sub_solutions) {
|
||||
// TODO: directly skip failures in mergeAnyCombination() or even earlier
|
||||
if (sub->isFailure())
|
||||
return;
|
||||
sub_trajectories.push_back(sub->trajectory());
|
||||
}
|
||||
|
||||
moveit::core::JointModelGroup *jmg = jmg_merged_.get();
|
||||
robot_trajectory::RobotTrajectoryPtr merged = task_constructor::merge(sub_trajectories, state, jmg);
|
||||
if (jmg_merged_.get() != jmg)
|
||||
jmg_merged_.reset(jmg);
|
||||
if (!merged) return;
|
||||
|
||||
SubTrajectory t(merged);
|
||||
// accumulate costs and markers
|
||||
double costs = 0.0;
|
||||
for (const auto& sub : sub_solutions) {
|
||||
costs += sub->cost();
|
||||
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
|
||||
}
|
||||
t.setCost(costs);
|
||||
spawner(std::move(t));
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -61,6 +61,7 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
||||
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
|
||||
p.declare<uint32_t>("max_ik_solutions", 1);
|
||||
p.declare<bool>("ignore_collisions", false);
|
||||
p.declare<std::set<std::string>>("forward_properties", "to-be-forwarded properties from input");
|
||||
|
||||
// ik_frame and target_pose are read from the interface
|
||||
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
|
||||
@ -68,19 +69,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
||||
p.configureInitFrom(Stage::INTERFACE, {"target_pose"});
|
||||
}
|
||||
|
||||
void ComputeIK::setTimeout(double timeout){
|
||||
setProperty("timeout", timeout);
|
||||
}
|
||||
|
||||
void ComputeIK::setEndEffector(const std::string &eef){
|
||||
setProperty("eef", eef);
|
||||
}
|
||||
|
||||
void ComputeIK::setIKFrame(const geometry_msgs::PoseStamped &pose)
|
||||
{
|
||||
setProperty("ik_frame", pose);
|
||||
}
|
||||
|
||||
void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
|
||||
{
|
||||
geometry_msgs::PoseStamped pose_msg;
|
||||
@ -89,11 +77,6 @@ void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
|
||||
setIKFrame(pose_msg);
|
||||
}
|
||||
|
||||
void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose)
|
||||
{
|
||||
setProperty("target_pose", pose);
|
||||
}
|
||||
|
||||
void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &frame)
|
||||
{
|
||||
geometry_msgs::PoseStamped pose_msg;
|
||||
@ -102,24 +85,16 @@ void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &fr
|
||||
setTargetPose(pose_msg);
|
||||
}
|
||||
|
||||
void ComputeIK::setMaxIKSolutions(uint32_t n){
|
||||
setProperty("max_ik_solutions", n);
|
||||
}
|
||||
|
||||
void ComputeIK::setIgnoreCollisions(bool flag)
|
||||
{
|
||||
setProperty("ignore_collisions", flag);
|
||||
}
|
||||
|
||||
// found IK solutions with a flag indicating validity
|
||||
typedef std::vector<std::vector<double>> IKSolutions;
|
||||
|
||||
namespace {
|
||||
|
||||
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
|
||||
// move into MoveIt! core
|
||||
// TODO: move into MoveIt! core, lift active_components_only_ from fcl to common interface
|
||||
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
|
||||
Eigen::Affine3d pose, const robot_model::LinkModel* link)
|
||||
Eigen::Affine3d pose, const robot_model::LinkModel* link,
|
||||
collision_detection::CollisionResult* collision_result = nullptr)
|
||||
{
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
|
||||
@ -150,11 +125,25 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
|
||||
|
||||
// check collision with the world using the padded version
|
||||
collision_detection::CollisionRequest req;
|
||||
collision_detection::CollisionResult res;
|
||||
collision_detection::CollisionResult result;
|
||||
req.contacts = (collision_result != nullptr);
|
||||
collision_detection::CollisionResult& res = collision_result ? *collision_result : result;
|
||||
scene->checkCollision(req, res, robot_state, acm);
|
||||
return res.collision;
|
||||
}
|
||||
|
||||
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap &contacts,
|
||||
const std::string& separator)
|
||||
{
|
||||
std::string result;
|
||||
for (const auto& contact : contacts) {
|
||||
if (!result.empty())
|
||||
result.append(separator);
|
||||
result.append(contact.first.first).append(" - ").append(contact.first.second);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool validateEEF(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model,
|
||||
const moveit::core::JointModelGroup*& jmg, std::string* msg)
|
||||
{
|
||||
@ -213,6 +202,9 @@ void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
|
||||
void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
{
|
||||
if (s.isFailure())
|
||||
return;
|
||||
|
||||
assert(s.start() && s.end());
|
||||
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
|
||||
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
|
||||
@ -283,9 +275,10 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
}
|
||||
|
||||
// validate placed link for collisions
|
||||
bool colliding = isTargetPoseColliding(sandbox_scene, target_pose, link);
|
||||
collision_detection::CollisionResult collisions;
|
||||
bool colliding = isTargetPoseColliding(sandbox_scene, target_pose, link, &collisions);
|
||||
if (colliding && !storeFailures()) {
|
||||
ROS_ERROR("eef in collision");
|
||||
ROS_WARN_STREAM("eef in collision: " << listCollisionPairs(collisions.contacts, "\n"));
|
||||
return;
|
||||
}
|
||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
||||
@ -308,7 +301,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
generateCollisionMarkers(sandbox_state, appender, link_to_visualize);
|
||||
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||
solution.setCost(std::numeric_limits<double>::infinity()); // mark solution as failure
|
||||
solution.setName("eef in collision");
|
||||
// TODO: visualize collisions
|
||||
solution.setName("eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
||||
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||
return;
|
||||
} else
|
||||
@ -377,7 +371,14 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
||||
robot_state.update();
|
||||
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
InterfaceState state(scene);
|
||||
const boost::any &forwards = props.get("forward_properties");
|
||||
if (!forwards.empty()) {
|
||||
auto p = s.start()->properties();
|
||||
p.exposeTo(state.properties(), boost::any_cast<std::set<std::string>>(forwards));
|
||||
}
|
||||
|
||||
spawn(std::move(state), std::move(solution));
|
||||
}
|
||||
|
||||
// TODO: magic constant should be a property instead ("current_seed_only", or equivalent)
|
||||
|
||||
@ -52,47 +52,17 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
|
||||
p.declare<std::string>("marker_ns", "", "marker namespace");
|
||||
p.declare<std::string>("group", "name of planning group");
|
||||
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
|
||||
p.declare<double>("min_distance", "minimum distance to move");
|
||||
p.declare<double>("max_distance", "maximum distance to move");
|
||||
p.declare<double>("min_distance", -1.0, "minimum distance to move");
|
||||
p.declare<double>("max_distance", 0.0, "maximum distance to move");
|
||||
|
||||
p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform");
|
||||
p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction");
|
||||
p.declare<std::map<std::string, double>>("joints", "Relative joint space goal");
|
||||
|
||||
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
||||
"constraints to maintain during trajectory");
|
||||
}
|
||||
|
||||
void MoveRelative::setGroup(const std::string& group){
|
||||
setProperty("group", group);
|
||||
}
|
||||
|
||||
void MoveRelative::setLink(const std::string& link){
|
||||
setProperty("link", link);
|
||||
}
|
||||
|
||||
void MoveRelative::setMinDistance(double distance){
|
||||
setProperty("min_distance", distance);
|
||||
}
|
||||
|
||||
void MoveRelative::setMaxDistance(double distance){
|
||||
setProperty("max_distance", distance);
|
||||
}
|
||||
|
||||
void MoveRelative::setMinMaxDistance(double min_distance, double max_distance){
|
||||
setProperty("min_distance", min_distance);
|
||||
setProperty("max_distance", max_distance);
|
||||
}
|
||||
|
||||
void MoveRelative::along(const geometry_msgs::TwistStamped &twist)
|
||||
{
|
||||
setProperty("twist", twist);
|
||||
}
|
||||
|
||||
void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction)
|
||||
{
|
||||
setProperty("direction", direction);
|
||||
}
|
||||
|
||||
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
PropagatingEitherWay::init(robot_model);
|
||||
@ -109,128 +79,172 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
const std::string& group = props.get<std::string>("group");
|
||||
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
|
||||
if (!jmg) {
|
||||
ROS_WARN_STREAM("MoveRelative: invalid joint model group: " << group);
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
||||
return false;
|
||||
}
|
||||
|
||||
// only allow single target
|
||||
size_t count_goals = props.countDefined({"twist", "direction"});
|
||||
size_t count_goals = props.countDefined({"twist", "direction", "joints"});
|
||||
if (count_goals != 1) {
|
||||
if (count_goals == 0) ROS_WARN("MoveRelative: no goal defined");
|
||||
else ROS_WARN("MoveRelative: cannot plan to multiple goals");
|
||||
if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined");
|
||||
else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Cartesian targets require the link name
|
||||
// TODO: use ik_frame property as in ComputeIK
|
||||
std::string link_name = props.get<std::string>("link");
|
||||
const moveit::core::LinkModel* link;
|
||||
if (link_name.empty())
|
||||
link_name = solvers::getEndEffectorLink(jmg);
|
||||
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
||||
ROS_WARN_STREAM("MoveRelative: no or invalid link name specified: " << link_name);
|
||||
return false;
|
||||
}
|
||||
|
||||
Property prop = props.property("max_distance");
|
||||
double max_distance = prop.value().empty() ? 0.0 : std::abs(boost::any_cast<double>(prop.value()));
|
||||
prop = props.property("min_distance");
|
||||
double min_distance = prop.value().empty() ? -1.0 : boost::any_cast<double>(prop.value());
|
||||
|
||||
bool use_rotation_distance = false; // measure achieved distance as rotation?
|
||||
Eigen::Vector3d linear; // linear translation
|
||||
Eigen::Vector3d angular; // angular rotation
|
||||
double linear_norm = 0.0, angular_norm = 0.0;
|
||||
|
||||
Eigen::Affine3d target_eigen;
|
||||
Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success
|
||||
|
||||
boost::any goal = props.get("twist");
|
||||
if (!goal.empty()) {
|
||||
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(goal);
|
||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||
tf::vectorMsgToEigen(target.twist.linear, linear);
|
||||
tf::vectorMsgToEigen(target.twist.angular, angular);
|
||||
|
||||
linear_norm = linear.norm();
|
||||
angular_norm = angular.norm();
|
||||
if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||
angular /= angular_norm; // normalize angular
|
||||
use_rotation_distance = linear_norm < std::numeric_limits<double>::epsilon();
|
||||
|
||||
// use max distance?
|
||||
if (max_distance > 0.0) {
|
||||
double scale;
|
||||
if (!use_rotation_distance) // non-zero linear motion defines distance
|
||||
scale = max_distance / linear_norm;
|
||||
else if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||
scale = max_distance / angular_norm;
|
||||
linear *= scale;
|
||||
linear_norm *= scale;
|
||||
angular_norm *= scale;
|
||||
}
|
||||
|
||||
// invert direction?
|
||||
if (dir == BACKWARD) {
|
||||
linear *= -1.0;
|
||||
angular *= -1.0;
|
||||
}
|
||||
|
||||
// compute absolute transform for link
|
||||
linear = frame_pose.linear() * linear;
|
||||
angular = frame_pose.linear() * angular;
|
||||
target_eigen = link_pose;
|
||||
target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
|
||||
target_eigen.translation() += linear;
|
||||
}
|
||||
|
||||
goal = props.get("direction");
|
||||
if (!goal.empty()) {
|
||||
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(goal);
|
||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||
tf::vectorMsgToEigen(target.vector, linear);
|
||||
|
||||
// use max distance?
|
||||
if (max_distance > 0.0) {
|
||||
linear.normalize();
|
||||
linear *= max_distance;
|
||||
}
|
||||
linear_norm = linear.norm();
|
||||
|
||||
// invert direction?
|
||||
if (dir == BACKWARD)
|
||||
linear *= -1.0;
|
||||
|
||||
// compute absolute transform for link
|
||||
linear = frame_pose.linear() * linear;
|
||||
target_eigen = link_pose;
|
||||
target_eigen.translation() += linear;
|
||||
}
|
||||
double max_distance = props.get<double>("max_distance");
|
||||
double min_distance = props.get<double>("min_distance");
|
||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||
bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory,
|
||||
props.get<moveit_msgs::Constraints>("path_constraints"));
|
||||
bool success = false;
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
double distance = 0.0;
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
||||
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||
boost::any goal = props.get("joints");
|
||||
if (!goal.empty()) {
|
||||
const auto& accepted = jmg->getJointModelNames();
|
||||
auto& robot_state = scene->getCurrentStateNonConst();
|
||||
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
||||
for (const auto& j : joints) {
|
||||
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
||||
auto jm = scene->getRobotModel()->getJointModel(index);
|
||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
|
||||
return false;
|
||||
}
|
||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||
robot_state.enforceBounds(jm);
|
||||
}
|
||||
success = distance >= min_distance;
|
||||
if (!success) {
|
||||
char msg[100];
|
||||
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
||||
trajectory.setName(msg);
|
||||
robot_state.update();
|
||||
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
} else {
|
||||
// Cartesian targets require the link name
|
||||
// TODO: use ik_frame property as in ComputeIK
|
||||
std::string link_name = props.get<std::string>("link");
|
||||
const moveit::core::LinkModel* link;
|
||||
if (link_name.empty())
|
||||
link_name = solvers::getEndEffectorLink(jmg);
|
||||
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "No or invalid link name specified: " << link_name);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool use_rotation_distance = false; // measure achieved distance as rotation?
|
||||
Eigen::Vector3d linear; // linear translation
|
||||
Eigen::Vector3d angular; // angular rotation
|
||||
double linear_norm = 0.0, angular_norm = 0.0;
|
||||
|
||||
Eigen::Affine3d target_eigen;
|
||||
Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success
|
||||
|
||||
boost::any goal = props.get("twist");
|
||||
if (!goal.empty()) {
|
||||
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(goal);
|
||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||
tf::vectorMsgToEigen(target.twist.linear, linear);
|
||||
tf::vectorMsgToEigen(target.twist.angular, angular);
|
||||
|
||||
linear_norm = linear.norm();
|
||||
angular_norm = angular.norm();
|
||||
if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||
angular /= angular_norm; // normalize angular
|
||||
use_rotation_distance = linear_norm < std::numeric_limits<double>::epsilon();
|
||||
|
||||
// use max distance?
|
||||
if (max_distance > 0.0) {
|
||||
double scale;
|
||||
if (!use_rotation_distance) // non-zero linear motion defines distance
|
||||
scale = max_distance / linear_norm;
|
||||
else if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||
scale = max_distance / angular_norm;
|
||||
linear *= scale;
|
||||
linear_norm *= scale;
|
||||
angular_norm *= scale;
|
||||
}
|
||||
|
||||
// invert direction?
|
||||
if (dir == BACKWARD) {
|
||||
linear *= -1.0;
|
||||
angular *= -1.0;
|
||||
}
|
||||
|
||||
// compute absolute transform for link
|
||||
linear = frame_pose.linear() * linear;
|
||||
angular = frame_pose.linear() * angular;
|
||||
target_eigen = link_pose;
|
||||
target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
|
||||
target_eigen.translation() += linear;
|
||||
}
|
||||
|
||||
goal = props.get("direction");
|
||||
if (!goal.empty()) {
|
||||
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(goal);
|
||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||
tf::vectorMsgToEigen(target.vector, linear);
|
||||
|
||||
// use max distance?
|
||||
if (max_distance > 0.0) {
|
||||
linear.normalize();
|
||||
linear *= max_distance;
|
||||
}
|
||||
linear_norm = linear.norm();
|
||||
|
||||
// invert direction?
|
||||
if (dir == BACKWARD)
|
||||
linear *= -1.0;
|
||||
|
||||
// compute absolute transform for link
|
||||
linear = frame_pose.linear() * linear;
|
||||
target_eigen = link_pose;
|
||||
target_eigen.translation() += linear;
|
||||
}
|
||||
|
||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
double distance = 0.0;
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
||||
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||
}
|
||||
success = distance >= min_distance;
|
||||
if (!success) {
|
||||
char msg[100];
|
||||
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
||||
trajectory.setName(msg);
|
||||
}
|
||||
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||
success = true;
|
||||
}
|
||||
|
||||
// add an arrow marker
|
||||
visualization_msgs::Marker m;
|
||||
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
|
||||
m.ns = props.get<std::string>("marker_ns");
|
||||
if (!m.ns.empty()) {
|
||||
m.header.frame_id = scene->getPlanningFrame();
|
||||
if (linear_norm > 1e-3) {
|
||||
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
|
||||
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
||||
: rviz_marker_tools::RED);
|
||||
rviz_marker_tools::makeArrow(m, linear_norm);
|
||||
auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
|
||||
Eigen::Vector3d pos(link_pose.translation());
|
||||
if (dir == BACKWARD) {
|
||||
// flip arrow direction
|
||||
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
|
||||
// arrow tip at goal_pose
|
||||
pos += quat * Eigen::Vector3d(-linear_norm, 0, 0);
|
||||
}
|
||||
tf::pointEigenToMsg(pos, m.pose.position);
|
||||
tf::quaternionEigenToMsg(quat, m.pose.orientation);
|
||||
trajectory.markers().push_back(m);
|
||||
}
|
||||
}
|
||||
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||
success = true;
|
||||
}
|
||||
|
||||
// store result
|
||||
@ -240,31 +254,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
trajectory.setTrajectory(robot_trajectory);
|
||||
}
|
||||
|
||||
// add an arrow marker
|
||||
visualization_msgs::Marker m;
|
||||
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
|
||||
m.ns = props.get<std::string>("marker_ns");
|
||||
if (!m.ns.empty()) {
|
||||
m.header.frame_id = scene->getPlanningFrame();
|
||||
if (linear_norm > 1e-3) {
|
||||
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
|
||||
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
||||
: rviz_marker_tools::RED);
|
||||
rviz_marker_tools::makeArrow(m, linear_norm);
|
||||
auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
|
||||
Eigen::Vector3d pos(link_pose.translation());
|
||||
if (dir == BACKWARD) {
|
||||
// flip arrow direction
|
||||
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
|
||||
// arrow tip at goal_pose
|
||||
pos += quat * Eigen::Vector3d(-linear_norm, 0, 0);
|
||||
}
|
||||
tf::pointEigenToMsg(pos, m.pose.position);
|
||||
tf::quaternionEigenToMsg(quat, m.pose.orientation);
|
||||
trajectory.markers().push_back(m);
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
@ -83,4 +83,10 @@ void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, doub
|
||||
p.set("max_distance", max_distance);
|
||||
}
|
||||
|
||||
void PickPlaceBase::setLiftPlace(const std::map<std::string, double>& joints)
|
||||
{
|
||||
auto& p = lift_stage_->properties();
|
||||
p.set("joints", joints);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user