MoveTo/MoveRelative: generic IK target frame

...instead of simple link name
This commit is contained in:
Robert Haschke 2018-05-24 13:56:18 +02:00 committed by v4hn
parent 495c80350b
commit 84dec07565
10 changed files with 117 additions and 99 deletions

View File

@ -46,7 +46,6 @@ int main(int argc, char** argv){
t.setProperty("group", std::string("left_arm"));
t.setProperty("eef", std::string("la_tool_mount"));
t.setProperty("gripper", std::string("left_hand"));
t.setProperty("link", std::string("la_tool_mount"));
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
@ -87,7 +86,7 @@ int main(int argc, char** argv){
move->restrictDirection(stages::MoveRelative::BACKWARD);
move->properties().configureInitFrom(Stage::PARENT);
move->properties().set("marker_ns", std::string("approach"));
move->properties().set("link", std::string("lh_tool_frame"));
move->setIKFrame("lh_tool_frame");
move->setMinMaxDistance(0.05, 0.1);
geometry_msgs::Vector3Stamped direction;
@ -142,7 +141,7 @@ int main(int argc, char** argv){
move->properties().configureInitFrom(Stage::PARENT, {"group"});
move->setMinMaxDistance(0.03, 0.05);
move->properties().set("marker_ns", std::string("lift"));
move->properties().set("link", std::string("lh_tool_frame"));
move->setIKFrame("lh_tool_frame");
geometry_msgs::Vector3Stamped direction;
direction.header.frame_id = "world";
@ -156,7 +155,7 @@ int main(int argc, char** argv){
move->properties().configureInitFrom(Stage::PARENT, {"group"});
move->setMinMaxDistance(0.1, 0.2);
move->properties().set("marker_ns", std::string("lift"));
move->properties().set("link", std::string("lh_tool_frame"));
move->setIKFrame("lh_tool_frame");
geometry_msgs::TwistStamped twist;
twist.header.frame_id = "object";

View File

@ -92,6 +92,4 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
};
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg);
} } }

View File

@ -58,8 +58,18 @@ public:
void setGroup(const std::string& group) {
setProperty("group", group);
}
void setLink(const std::string& link) {
setProperty("link", link);
/// setters for IK frame
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) {
Eigen::Affine3d pose; pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Affine3d::Identity(), link);
}
/// set minimum / maximum distance to move

View File

@ -60,8 +60,18 @@ public:
void setGroup(const std::string& group) {
setProperty("group", group);
}
void setLink(const std::string& link) {
setProperty("link", link);
/// setters for IK frame
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) {
Eigen::Affine3d pose; pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Affine3d::Identity(), link);
}
/// move link to given pose

View File

@ -25,7 +25,6 @@ add_library(${PROJECT_NAME}
storage.cpp
task.cpp
solvers/planner_interface.cpp
solvers/cartesian_path.cpp
solvers/pipeline_planner.cpp
)

View File

@ -63,9 +63,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints)
{
const moveit::core::LinkModel* link;
const std::string& link_name = solvers::getEndEffectorLink(jmg);
if (!(link = from->getRobotModel()->getLinkModel(link_name))) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) {
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
return false;
}

View File

@ -1,51 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: planner interface
*/
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/robot_model/joint_model_group.h>
namespace moveit { namespace task_constructor { namespace solvers {
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg)
{
// TODO: directly return LinkModel*
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
return link ? link->getName() : std::string();
}
} } }

View File

@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke
/* Authors: Robert Haschke, Michael Goerner
Desc: Move link along Cartesian direction
*/
@ -49,9 +49,10 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
{
auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout");
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
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<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<double>("min_distance", -1.0, "minimum distance to move");
p.declare<double>("max_distance", 0.0, "maximum distance to move");
@ -63,6 +64,14 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
"constraints to maintain during trajectory");
}
void MoveRelative::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);
setIKFrame(pose_msg);
}
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
{
PropagatingEitherWay::init(robot_model);
@ -72,12 +81,13 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &trajectory, Direction dir) {
scene = state.scene()->diff();
assert(scene->getRobotModel());
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
assert(robot_model);
const auto& props = properties();
double timeout = props.get<double>("timeout");
const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
return false;
@ -105,7 +115,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
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);
auto jm = robot_model->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;
@ -116,16 +126,25 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
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");
// Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg;
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);
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Failed to derive IK target link");
return false;
}
ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0;
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Unknown link: " << ik_pose_msg.header.frame_id);
return false;
}
}
bool use_rotation_distance = false; // measure achieved distance as rotation?
Eigen::Vector3d linear; // linear translation
@ -133,7 +152,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
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
Eigen::Affine3d link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
boost::any goal = props.get("twist");
if (!goal.empty()) {
@ -197,6 +216,11 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
target_eigen.translation() += linear;
}
// transform target pose such that ik frame will reach there if link does
Eigen::Affine3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse();
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
// min_distance reached?
@ -223,7 +247,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
// 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();

View File

@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke
/* Authors: Robert Haschke, Michael Goerner
Desc: Move to joint-state or Cartesian goal pose
*/
@ -51,7 +51,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose");
p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point");
@ -62,6 +62,14 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
"constraints to maintain during trajectory");
}
void MoveTo::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);
setIKFrame(pose_msg);
}
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
{
PropagatingEitherWay::init(robot_model);
@ -114,12 +122,13 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) {
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &solution, Direction dir) {
scene = state.scene()->diff();
assert(scene->getRobotModel());
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
assert(robot_model);
const auto& props = properties();
double timeout = props.get<double>("timeout");
const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) {
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group);
return false;
@ -161,15 +170,24 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
const moveit::core::LinkModel* link;
Eigen::Affine3d target_eigen;
// Cartesian targets require the link name
// TODO: use ik_frame property as in ComputeIK
std::string link_name = props.get<std::string>("link");
if (link_name.empty())
link_name = solvers::getEndEffectorLink(jmg);
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
ROS_WARN_STREAM_NAMED("MoveTo", "No or invalid link name specified: " << link_name);
// Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link");
return false;
}
ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0;
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
ROS_WARN_STREAM_NAMED("MoveTo", "Unknown link: " << ik_pose_msg.header.frame_id);
return false;
}
}
boost::any goal = props.get("pose");
if (!goal.empty()) {
@ -184,10 +202,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
// frame at link
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link_name;
pose_msg.pose.orientation.w = 1.0;
rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
}
goal = props.get("point");
@ -201,10 +216,15 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
target_point = frame * target_point;
// retain link orientation
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name);
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
target_eigen.translation() = target_point;
}
// transform target pose such that ik frame will reach there if link does
Eigen::Affine3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse();
// plan to Cartesian target
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
}

View File

@ -24,11 +24,22 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
auto init_ik_frame = [](const PropertyMap& other) -> boost::any {
geometry_msgs::PoseStamped pose;
const boost::any& frame = other.get("eef_frame");
if (frame.empty())
return boost::any();
pose.header.frame_id = boost::any_cast<std::string>("eef_frame");
pose.pose.orientation.w = 1.0;
return pose;
};
{
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.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame);
p.set("marker_ns", std::string(forward ? "approach" : "retract"));
approach_stage_ = approach.get();
insert(std::move(approach), insertion_position);
@ -42,7 +53,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
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.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame);
p.set("marker_ns", std::string(forward ? "lift" : "place"));
lift_stage_ = lift.get();
insert(std::move(lift), insertion_position);