consider reference_frame

This commit is contained in:
Robert Haschke 2018-03-24 21:12:05 +01:00
parent 7531b8ad54
commit f497112513
2 changed files with 39 additions and 26 deletions

View File

@ -43,10 +43,10 @@ public:
template <typename T> template <typename T>
void setReferenceFrame(const T& p, const std::string& frame = "") { void setReferenceFrame(const T& p, const std::string& frame = "") {
Eigen::Affine3d pose; pose = p; Eigen::Affine3d pose; pose = p;
setTargetPose(pose, frame); setReferenceFrame(pose, frame);
} }
void setReferenceFrame(const std::string& frame) { void setReferenceFrame(const std::string& frame) {
setTargetPose(Eigen::Affine3d::Identity(), frame); setReferenceFrame(Eigen::Affine3d::Identity(), frame);
} }
/// setters for target pose /// setters for target pose

View File

@ -88,13 +88,12 @@ namespace {
bool isTargetPoseColliding(const planning_scene::PlanningSceneConstPtr& scene, bool isTargetPoseColliding(const planning_scene::PlanningSceneConstPtr& scene,
const robot_model::JointModelGroup* jmg, const robot_model::JointModelGroup* jmg,
Eigen::Affine3d pose, Eigen::Affine3d pose,
const std::string &link_name) const robot_model::LinkModel* link)
{ {
planning_scene::PlanningScenePtr sandbox_scene = scene->diff(); planning_scene::PlanningScenePtr sandbox_scene = scene->diff();
robot_state::RobotState& robot_state = sandbox_scene->getCurrentStateNonConst(); robot_state::RobotState& robot_state = sandbox_scene->getCurrentStateNonConst();
// consider all rigidly connected parent links as well // consider all rigidly connected parent links as well
const robot_model::LinkModel* link = robot_state.getLinkModel(link_name);
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
if (parent != link) // transform pose into pose suitable to place parent if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent); pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
@ -209,37 +208,54 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined"); ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined");
return; return;
} }
const robot_model::LinkModel* link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second)
: jmg->getOnlyOneEndEffectorTip(); // extract target_pose
if (!link) { geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose");
if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame
target_pose_msg.header.frame_id = sandbox_scene->getPlanningFrame();
Eigen::Affine3d target_pose;
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
return;
}
// transform target_pose w.r.t. planning frame
target_pose = sandbox_scene->getFrameTransform(target_pose_msg.header.frame_id) * target_pose;
}
// determine IK link from reference_frame
const robot_model::LinkModel* link = nullptr;
const boost::any& value = props.get("reference_frame");
if (value.empty()) { // property undefined
// determine IK link from eef/group
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second)
: jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link"); ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
return; return;
} }
const std::string& link_name = link->getName(); } else {
geometry_msgs::PoseStamped ref_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst(); Eigen::Affine3d ref_pose;
tf::poseMsgToEigen(ref_pose_msg.pose, ref_pose);
// compute target pose w.r.t. link_name if (!(link = robot_model->getLinkModel(ref_pose_msg.header.frame_id))) {
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose"); ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown link: " << ref_pose_msg.header.frame_id);
Eigen::Affine3d target_pose; return;
tf::poseMsgToEigen(target_pose_msg.pose, target_pose); }
if (!target_pose_msg.header.frame_id.empty() && target_pose_msg.header.frame_id != link_name) { // transform target pose such that reference frame will reach there if link does
const robot_model::LinkModel* ref_link = robot_model->getLinkModel(target_pose_msg.header.frame_id); target_pose = target_pose * ref_pose.inverse();
if (!ref_link) throw std::runtime_error("requested reference frame '" + target_pose_msg.header.frame_id + "' is not a robot link");
const Eigen::Affine3d link_pose = sandbox_state.getGlobalLinkTransform(link_name);
const Eigen::Affine3d ref_pose = sandbox_state.getGlobalLinkTransform(target_pose_msg.header.frame_id);
// transform target pose such that the link frame will reach there
target_pose = target_pose * ref_pose.inverse() * link_pose;
} }
// validate placed link for collisions // validate placed link for collisions
bool colliding = isTargetPoseColliding(sandbox_scene, eef_jmg, target_pose, link_name); bool colliding = isTargetPoseColliding(sandbox_scene, eef_jmg, target_pose, link);
if (colliding && !storeFailures()) { if (colliding && !storeFailures()) {
ROS_ERROR("eef in collision"); ROS_ERROR("eef in collision");
return; return;
} }
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
// visualize placed end-effector // visualize placed end-effector
std::deque<visualization_msgs::Marker> placed_link_markers; std::deque<visualization_msgs::Marker> placed_link_markers;
auto appender = [&placed_link_markers](visualization_msgs::Marker& marker, const std::string& name) { auto appender = [&placed_link_markers](visualization_msgs::Marker& marker, const std::string& name) {
@ -271,9 +287,6 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
} else } else
sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose); sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
// prepare for marker frame usage
target_pose_msg.header.frame_id = s.start()->scene()->getPlanningFrame();
IKSolutions ik_solutions; IKSolutions ik_solutions;
bool ignore_collisions = props.get<bool>("ignore_collisions"); bool ignore_collisions = props.get<bool>("ignore_collisions");
auto isValid = [sandbox_scene, ignore_collisions, &ik_solutions] auto isValid = [sandbox_scene, ignore_collisions, &ik_solutions]
@ -300,7 +313,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
tried_current_state_as_seed= true; tried_current_state_as_seed= true;
size_t previous = ik_solutions.size(); size_t previous = ik_solutions.size();
bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link_name, 1, remaining_time, isValid); bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link->getName(), 1, remaining_time, isValid);
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count(); remaining_time -= std::chrono::duration<double>(now - start_time).count();