mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo/MoveRelative: generic IK target frame
...instead of simple link name
This commit is contained in:
parent
495c80350b
commit
84dec07565
@ -46,7 +46,6 @@ int main(int argc, char** argv){
|
|||||||
t.setProperty("group", std::string("left_arm"));
|
t.setProperty("group", std::string("left_arm"));
|
||||||
t.setProperty("eef", std::string("la_tool_mount"));
|
t.setProperty("eef", std::string("la_tool_mount"));
|
||||||
t.setProperty("gripper", std::string("left_hand"));
|
t.setProperty("gripper", std::string("left_hand"));
|
||||||
t.setProperty("link", std::string("la_tool_mount"));
|
|
||||||
|
|
||||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||||
pipeline->setTimeout(8.0);
|
pipeline->setTimeout(8.0);
|
||||||
@ -87,7 +86,7 @@ int main(int argc, char** argv){
|
|||||||
move->restrictDirection(stages::MoveRelative::BACKWARD);
|
move->restrictDirection(stages::MoveRelative::BACKWARD);
|
||||||
move->properties().configureInitFrom(Stage::PARENT);
|
move->properties().configureInitFrom(Stage::PARENT);
|
||||||
move->properties().set("marker_ns", std::string("approach"));
|
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);
|
move->setMinMaxDistance(0.05, 0.1);
|
||||||
|
|
||||||
geometry_msgs::Vector3Stamped direction;
|
geometry_msgs::Vector3Stamped direction;
|
||||||
@ -142,7 +141,7 @@ int main(int argc, char** argv){
|
|||||||
move->properties().configureInitFrom(Stage::PARENT, {"group"});
|
move->properties().configureInitFrom(Stage::PARENT, {"group"});
|
||||||
move->setMinMaxDistance(0.03, 0.05);
|
move->setMinMaxDistance(0.03, 0.05);
|
||||||
move->properties().set("marker_ns", std::string("lift"));
|
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;
|
geometry_msgs::Vector3Stamped direction;
|
||||||
direction.header.frame_id = "world";
|
direction.header.frame_id = "world";
|
||||||
@ -156,7 +155,7 @@ int main(int argc, char** argv){
|
|||||||
move->properties().configureInitFrom(Stage::PARENT, {"group"});
|
move->properties().configureInitFrom(Stage::PARENT, {"group"});
|
||||||
move->setMinMaxDistance(0.1, 0.2);
|
move->setMinMaxDistance(0.1, 0.2);
|
||||||
move->properties().set("marker_ns", std::string("lift"));
|
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;
|
geometry_msgs::TwistStamped twist;
|
||||||
twist.header.frame_id = "object";
|
twist.header.frame_id = "object";
|
||||||
|
|||||||
@ -92,6 +92,4 @@ public:
|
|||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg);
|
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -58,8 +58,18 @@ public:
|
|||||||
void setGroup(const std::string& group) {
|
void setGroup(const std::string& group) {
|
||||||
setProperty("group", group);
|
setProperty("group", group);
|
||||||
}
|
}
|
||||||
void setLink(const std::string& link) {
|
/// setters for IK frame
|
||||||
setProperty("link", link);
|
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
|
/// set minimum / maximum distance to move
|
||||||
|
|||||||
@ -60,8 +60,18 @@ public:
|
|||||||
void setGroup(const std::string& group) {
|
void setGroup(const std::string& group) {
|
||||||
setProperty("group", group);
|
setProperty("group", group);
|
||||||
}
|
}
|
||||||
void setLink(const std::string& link) {
|
/// setters for IK frame
|
||||||
setProperty("link", link);
|
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
|
/// move link to given pose
|
||||||
|
|||||||
@ -25,7 +25,6 @@ add_library(${PROJECT_NAME}
|
|||||||
storage.cpp
|
storage.cpp
|
||||||
task.cpp
|
task.cpp
|
||||||
|
|
||||||
solvers/planner_interface.cpp
|
|
||||||
solvers/cartesian_path.cpp
|
solvers/cartesian_path.cpp
|
||||||
solvers/pipeline_planner.cpp
|
solvers/pipeline_planner.cpp
|
||||||
)
|
)
|
||||||
|
|||||||
@ -63,9 +63,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints)
|
const moveit_msgs::Constraints& path_constraints)
|
||||||
{
|
{
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
|
||||||
const std::string& link_name = solvers::getEndEffectorLink(jmg);
|
if (!link) {
|
||||||
if (!(link = from->getRobotModel()->getLinkModel(link_name))) {
|
|
||||||
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
|
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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();
|
|
||||||
}
|
|
||||||
|
|
||||||
} } }
|
|
||||||
@ -32,7 +32,7 @@
|
|||||||
* POSSIBILITY OF SUCH DAMAGE.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
|
|
||||||
/* Authors: Michael Goerner, Robert Haschke
|
/* Authors: Robert Haschke, Michael Goerner
|
||||||
Desc: Move link along Cartesian direction
|
Desc: Move link along Cartesian direction
|
||||||
*/
|
*/
|
||||||
|
|
||||||
@ -49,9 +49,10 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
|
|||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", 10.0, "planning timeout");
|
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>("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<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>("min_distance", -1.0, "minimum distance to move");
|
||||||
p.declare<double>("max_distance", 0.0, "maximum 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");
|
"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)
|
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(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,
|
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||||
SubTrajectory &trajectory, Direction dir) {
|
SubTrajectory &trajectory, Direction dir) {
|
||||||
scene = state.scene()->diff();
|
scene = state.scene()->diff();
|
||||||
assert(scene->getRobotModel());
|
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||||
|
assert(robot_model);
|
||||||
|
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
double timeout = props.get<double>("timeout");
|
double timeout = props.get<double>("timeout");
|
||||||
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 = robot_model->getJointModelGroup(group);
|
||||||
if (!jmg) {
|
if (!jmg) {
|
||||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
||||||
return false;
|
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);
|
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
||||||
for (const auto& j : joints) {
|
for (const auto& j : joints) {
|
||||||
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
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()) {
|
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 << "'");
|
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
|
||||||
return false;
|
return false;
|
||||||
@ -116,15 +126,24 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
robot_state.update();
|
robot_state.update();
|
||||||
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
} else {
|
} else {
|
||||||
// Cartesian targets require the link name
|
// Cartesian targets require an IK reference frame
|
||||||
// TODO: use ik_frame property as in ComputeIK
|
geometry_msgs::PoseStamped ik_pose_msg;
|
||||||
std::string link_name = props.get<std::string>("link");
|
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
if (link_name.empty())
|
const boost::any& value = props.get("ik_frame");
|
||||||
link_name = solvers::getEndEffectorLink(jmg);
|
if (value.empty()) { // property undefined
|
||||||
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
// determine IK link from group
|
||||||
ROS_WARN_STREAM_NAMED("MoveRelative", "No or invalid link name specified: " << link_name);
|
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
|
||||||
return false;
|
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?
|
bool use_rotation_distance = false; // measure achieved distance as rotation?
|
||||||
@ -133,7 +152,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
double linear_norm = 0.0, angular_norm = 0.0;
|
double linear_norm = 0.0, angular_norm = 0.0;
|
||||||
|
|
||||||
Eigen::Affine3d target_eigen;
|
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");
|
boost::any goal = props.get("twist");
|
||||||
if (!goal.empty()) {
|
if (!goal.empty()) {
|
||||||
@ -197,6 +216,11 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
target_eigen.translation() += linear;
|
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);
|
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
|
|
||||||
// min_distance reached?
|
// min_distance reached?
|
||||||
@ -223,7 +247,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
|
|
||||||
// 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"
|
|
||||||
m.ns = props.get<std::string>("marker_ns");
|
m.ns = props.get<std::string>("marker_ns");
|
||||||
if (!m.ns.empty()) {
|
if (!m.ns.empty()) {
|
||||||
m.header.frame_id = scene->getPlanningFrame();
|
m.header.frame_id = scene->getPlanningFrame();
|
||||||
|
|||||||
@ -32,7 +32,7 @@
|
|||||||
* POSSIBILITY OF SUCH DAMAGE.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
|
|
||||||
/* Authors: Michael Goerner, Robert Haschke
|
/* Authors: Robert Haschke, Michael Goerner
|
||||||
Desc: Move to joint-state or Cartesian goal pose
|
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();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage
|
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>("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::PoseStamped>("pose", "Cartesian target pose");
|
||||||
p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point");
|
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");
|
"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)
|
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(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,
|
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||||
SubTrajectory &solution, Direction dir) {
|
SubTrajectory &solution, Direction dir) {
|
||||||
scene = state.scene()->diff();
|
scene = state.scene()->diff();
|
||||||
assert(scene->getRobotModel());
|
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||||
|
assert(robot_model);
|
||||||
|
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
double timeout = props.get<double>("timeout");
|
double timeout = props.get<double>("timeout");
|
||||||
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 = robot_model->getJointModelGroup(group);
|
||||||
if (!jmg) {
|
if (!jmg) {
|
||||||
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group);
|
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group);
|
||||||
return false;
|
return false;
|
||||||
@ -161,14 +170,23 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
|||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
Eigen::Affine3d target_eigen;
|
Eigen::Affine3d target_eigen;
|
||||||
|
|
||||||
// Cartesian targets require the link name
|
// Cartesian targets require an IK reference frame
|
||||||
// TODO: use ik_frame property as in ComputeIK
|
geometry_msgs::PoseStamped ik_pose_msg;
|
||||||
std::string link_name = props.get<std::string>("link");
|
const boost::any& value = props.get("ik_frame");
|
||||||
if (link_name.empty())
|
if (value.empty()) { // property undefined
|
||||||
link_name = solvers::getEndEffectorLink(jmg);
|
// determine IK link from group
|
||||||
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
|
||||||
ROS_WARN_STREAM_NAMED("MoveTo", "No or invalid link name specified: " << link_name);
|
ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link");
|
||||||
return false;
|
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");
|
boost::any goal = props.get("pose");
|
||||||
@ -184,10 +202,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
|||||||
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
|
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
|
||||||
|
|
||||||
// frame at link
|
// frame at link
|
||||||
geometry_msgs::PoseStamped pose_msg;
|
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
|
||||||
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");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
goal = props.get("point");
|
goal = props.get("point");
|
||||||
@ -201,10 +216,15 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
|||||||
target_point = frame * target_point;
|
target_point = frame * target_point;
|
||||||
|
|
||||||
// retain link orientation
|
// retain link orientation
|
||||||
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name);
|
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
|
||||||
target_eigen.translation() = target_point;
|
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
|
// plan to Cartesian target
|
||||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -24,11 +24,22 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
|
|||||||
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
|
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
|
||||||
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
|
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_);
|
auto approach = std::make_unique<MoveRelative>(forward ? "approach object" : "retract", cartesian_solver_);
|
||||||
PropertyMap& p = approach->properties();
|
PropertyMap& p = approach->properties();
|
||||||
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
|
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"));
|
p.set("marker_ns", std::string(forward ? "approach" : "retract"));
|
||||||
approach_stage_ = approach.get();
|
approach_stage_ = approach.get();
|
||||||
insert(std::move(approach), insertion_position);
|
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_);
|
auto lift = std::make_unique<MoveRelative>(forward ? "lift object" : "place object", cartesian_solver_);
|
||||||
PropertyMap& p = lift->properties();
|
PropertyMap& p = lift->properties();
|
||||||
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
|
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"));
|
p.set("marker_ns", std::string(forward ? "lift" : "place"));
|
||||||
lift_stage_ = lift.get();
|
lift_stage_ = lift.get();
|
||||||
insert(std::move(lift), insertion_position);
|
insert(std::move(lift), insertion_position);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user