mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
consider reference_frame
This commit is contained in:
parent
7531b8ad54
commit
f497112513
@ -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
|
||||||
|
|||||||
@ -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();
|
|
||||||
if (!link) {
|
|
||||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
const std::string& link_name = link->getName();
|
|
||||||
|
|
||||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
// extract target_pose
|
||||||
|
|
||||||
// compute target pose w.r.t. link_name
|
|
||||||
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose");
|
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;
|
Eigen::Affine3d target_pose;
|
||||||
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
|
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
|
||||||
if (!target_pose_msg.header.frame_id.empty() && target_pose_msg.header.frame_id != link_name) {
|
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
|
||||||
const robot_model::LinkModel* ref_link = robot_model->getLinkModel(target_pose_msg.header.frame_id);
|
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
|
||||||
if (!ref_link) throw std::runtime_error("requested reference frame '" + target_pose_msg.header.frame_id + "' is not a robot link");
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
const Eigen::Affine3d link_pose = sandbox_state.getGlobalLinkTransform(link_name);
|
// determine IK link from reference_frame
|
||||||
const Eigen::Affine3d ref_pose = sandbox_state.getGlobalLinkTransform(target_pose_msg.header.frame_id);
|
const robot_model::LinkModel* link = nullptr;
|
||||||
// transform target pose such that the link frame will reach there
|
const boost::any& value = props.get("reference_frame");
|
||||||
target_pose = target_pose * ref_pose.inverse() * link_pose;
|
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");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
geometry_msgs::PoseStamped ref_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
|
||||||
|
Eigen::Affine3d ref_pose;
|
||||||
|
tf::poseMsgToEigen(ref_pose_msg.pose, ref_pose);
|
||||||
|
if (!(link = robot_model->getLinkModel(ref_pose_msg.header.frame_id))) {
|
||||||
|
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown link: " << ref_pose_msg.header.frame_id);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// transform target pose such that reference frame will reach there if link does
|
||||||
|
target_pose = target_pose * ref_pose.inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user