This commit is contained in:
Robert Haschke 2018-04-15 20:38:03 +02:00
parent 9a7aa4dad7
commit 40ca35085a
5 changed files with 196 additions and 218 deletions

View File

@ -296,6 +296,7 @@ std::ostream& operator<<(std::ostream& os, const std::map<std::string, T>& m) {
first = false; first = false;
} }
os << "}"; os << "}";
return os;
} }
} // namespace task_constructor } // namespace task_constructor

View File

@ -55,24 +55,42 @@ public:
bool computeForward(const InterfaceState& from) override; bool computeForward(const InterfaceState& from) override;
bool computeBackward(const InterfaceState& to) override; bool computeBackward(const InterfaceState& to) override;
void setGroup(const std::string& group); void setGroup(const std::string& group) {
void setLink(const std::string& link); setProperty("group", group);
}
void setLink(const std::string& link) {
setProperty("link", link);
}
/// set minimum / maximum distance to move /// set minimum / maximum distance to move
void setMinDistance(double distance); void setMinDistance(double distance) {
void setMaxDistance(double distance); setProperty("min_distance", distance);
void setMinMaxDistance(double min_distance, double max_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 setRelativeJointSpaceGoal(const std::map<std::string, double>& goal); void setPathConstraints(moveit_msgs::Constraints path_constraints) {
void setPathConstraints(moveit_msgs::Constraints path_constraints){
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }
/// perform twist motion on specified link /// 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 /// 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: protected:
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
@ -80,7 +98,6 @@ protected:
protected: protected:
solvers::PlannerInterfacePtr planner_; solvers::PlannerInterfacePtr planner_;
bool joint_space_goal_ = false;
}; };
} } } } } }

View File

@ -86,10 +86,10 @@ public:
void setApproachRetract(const geometry_msgs::TwistStamped& motion, void setApproachRetract(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance); double min_distance, double max_distance);
void setLiftPlace(const geometry_msgs::TwistStamped& motion, void setLiftPlace(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance); double min_distance, double max_distance);
void setLiftPlace(const std::map<std::string, double>& joints);
void setRelativeJointSpaceGoal(const std::map<std::string, double>& relative_joint_space_goal);
}; };
@ -109,9 +109,8 @@ public:
double min_distance, double max_distance) { double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance); setLiftPlace(motion, min_distance, max_distance);
} }
void setLiftMotion(const std::map<std::string, double>& joints) {
void setRelativeJointSpaceLiftGoal(const std::map<std::string, double>& relative_joint_space_goal) { setLiftPlace(joints);
setRelativeJointSpaceGoal(relative_joint_space_goal);
} }
}; };
@ -132,6 +131,9 @@ public:
double min_distance, double max_distance) { double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance); setLiftPlace(motion, min_distance, max_distance);
} }
void setPlaceMotion(const std::map<std::string, double>& joints) {
setLiftPlace(joints);
}
}; };

View File

@ -52,52 +52,17 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
p.declare<std::string>("marker_ns", "", "marker namespace"); p.declare<std::string>("marker_ns", "", "marker namespace");
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "link to move (default is tip of jmg)"); 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>("min_distance", -1.0, "minimum distance to move");
p.declare<double>("max_distance", "maximum 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::TwistStamped>("twist", "Cartesian twist transform");
p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction"); p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction");
p.declare<std::map<std::string, double>>("relative_joint_space_goal", "Relative joint space goal"); p.declare<std::map<std::string, double>>("joints", "Relative joint space goal");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(), p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory"); "constraints to maintain during trajectory");
} }
void MoveRelative::setRelativeJointSpaceGoal(const std::map<std::string, double>& goal){
setProperty("relative_joint_space_goal", goal);
}
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) void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
PropagatingEitherWay::init(robot_model); PropagatingEitherWay::init(robot_model);
@ -114,18 +79,43 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
const std::string& group = props.get<std::string>("group"); const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
if (!jmg) { if (!jmg) {
ROS_WARN_STREAM("MoveRelative: invalid joint model group: " << group); ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
return false; return false;
} }
// only allow single target // only allow single target
size_t count_goals = props.countDefined({"twist", "direction", "relative_joint_space_goal"}); size_t count_goals = props.countDefined({"twist", "direction", "joints"});
if (count_goals != 1) { if (count_goals != 1) {
if (count_goals == 0) ROS_WARN("MoveRelative: no goal defined"); if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined");
else ROS_WARN("MoveRelative: cannot plan to multiple goals"); else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals");
return false; return false;
} }
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 = false;
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 (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", "Ignoring joint " << j.first << " that is not part of group " << group);
continue;
}
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
robot_state.enforceBounds(jm);
}
robot_state.update();
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else {
// Cartesian targets require the link name // Cartesian targets require the link name
// TODO: use ik_frame property as in ComputeIK // TODO: use ik_frame property as in ComputeIK
std::string link_name = props.get<std::string>("link"); std::string link_name = props.get<std::string>("link");
@ -133,15 +123,10 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
if (link_name.empty()) if (link_name.empty())
link_name = solvers::getEndEffectorLink(jmg); link_name = solvers::getEndEffectorLink(jmg);
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
ROS_WARN_STREAM("MoveRelative: no or invalid link name specified: " << link_name); ROS_WARN_STREAM_NAMED("MoveRelative", "No or invalid link name specified: " << link_name);
return false; 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? bool use_rotation_distance = false; // measure achieved distance as rotation?
Eigen::Vector3d linear; // linear translation Eigen::Vector3d linear; // linear translation
Eigen::Vector3d angular; // angular rotation Eigen::Vector3d angular; // angular rotation
@ -212,35 +197,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
target_eigen.translation() += linear; target_eigen.translation() += linear;
} }
goal = props.get("relative_joint_space_goal"); success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
if (!goal.empty()) {
const auto &start_state = scene->getCurrentState();
auto& current_state = scene->getCurrentStateNonConst();
joint_space_goal_ = true;
const std::map<std::string, double>& relative_joint_position_map = boost::any_cast<std::map<std::string, double>>(goal);
for (auto j : relative_joint_position_map) {
double new_pos = (start_state.getVariablePosition(j.first) + j.second);
current_state.setVariablePosition(j.first, new_pos);
auto jm = scene->getRobotModel()->getJointModel(j.first);
current_state.enforceBounds(jm);
current_state.update(true);
}
}
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false;
if (joint_space_goal_) {
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory,
props.get<moveit_msgs::Constraints>("path_constraints"));
} else {
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory,
props.get<moveit_msgs::Constraints>("path_constraints"));
}
// min_distance reached? // min_distance reached?
if (min_distance > 0.0) { if (min_distance > 0.0) {
@ -264,13 +221,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
success = true; success = true;
} }
// store result
if (robot_trajectory && (success || storeFailures())) {
scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD) robot_trajectory->reverse();
trajectory.setTrajectory(robot_trajectory);
}
// add an arrow marker // add an arrow marker
visualization_msgs::Marker m; visualization_msgs::Marker m;
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
@ -295,6 +245,14 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
trajectory.markers().push_back(m); trajectory.markers().push_back(m);
} }
} }
}
// store result
if (robot_trajectory && (success || storeFailures())) {
scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD) robot_trajectory->reverse();
trajectory.setTrajectory(robot_trajectory);
}
return success; return success;
} }

View File

@ -83,10 +83,10 @@ void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, doub
p.set("max_distance", max_distance); p.set("max_distance", max_distance);
} }
void PickPlaceBase::setRelativeJointSpaceGoal(const std::map<std::string, double>& relative_joint_space_goal) void PickPlaceBase::setLiftPlace(const std::map<std::string, double>& joints)
{ {
auto& p = lift_stage_->properties(); auto& p = lift_stage_->properties();
p.set("relative_joint_space_goal", relative_joint_space_goal); p.set("joints", joints);
} }
} } } } } }