mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
do not modify scene in isTargetPoseColliding
Especially, do not create a custom scene at all. The method only affects a RobotState. Fixes https://github.com/ros-planning/moveit_task_constructor/issues/209
This commit is contained in:
parent
74d33c4ec0
commit
ff86217f9a
@ -89,22 +89,21 @@ namespace {
|
||||
|
||||
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
|
||||
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
|
||||
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d pose,
|
||||
const robot_model::LinkModel* link,
|
||||
collision_detection::CollisionResult* collision_result = nullptr) {
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
|
||||
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
|
||||
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
|
||||
const robot_model::LinkModel* link,
|
||||
collision_detection::CollisionResult* collision_result = nullptr) {
|
||||
// consider all rigidly connected parent links as well
|
||||
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
|
||||
if (parent != link) // transform pose into pose suitable to place parent
|
||||
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
|
||||
|
||||
// place link at given pose
|
||||
// place links at given pose
|
||||
robot_state.updateStateWithLinkAt(parent, pose);
|
||||
robot_state.updateCollisionBodyTransforms();
|
||||
|
||||
// disable collision checking for parent links (except links fixed to root)
|
||||
auto& acm = scene->getAllowedCollisionMatrixNonConst();
|
||||
auto acm = scene->getAllowedCollisionMatrix();
|
||||
std::vector<const std::string*> pending_links; // parent link names that might be rigidly connected to root
|
||||
while (parent) {
|
||||
pending_links.push_back(&parent->getName());
|
||||
@ -231,10 +230,10 @@ void ComputeIK::compute() {
|
||||
properties().performInitFrom(INTERFACE, s.start()->properties());
|
||||
const auto& props = properties();
|
||||
|
||||
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
|
||||
const planning_scene::PlanningSceneConstPtr& scene{ s.start()->scene() };
|
||||
|
||||
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
||||
const auto& robot_model = sandbox_scene->getRobotModel();
|
||||
const auto& robot_model = scene->getRobotModel();
|
||||
const moveit::core::JointModelGroup* eef_jmg = nullptr;
|
||||
const moveit::core::JointModelGroup* jmg = nullptr;
|
||||
std::string msg;
|
||||
@ -256,18 +255,18 @@ void ComputeIK::compute() {
|
||||
// extract 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();
|
||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
||||
|
||||
Eigen::Isometry3d 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)) {
|
||||
if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
|
||||
if (!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;
|
||||
target_pose = scene->getFrameTransform(target_pose_msg.header.frame_id) * target_pose;
|
||||
}
|
||||
|
||||
// determine IK link from ik_frame
|
||||
@ -291,7 +290,7 @@ void ComputeIK::compute() {
|
||||
link = robot_model->getLinkModel(ik_pose_msg.header.frame_id);
|
||||
} else {
|
||||
const robot_state::AttachedBody* attached =
|
||||
sandbox_scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
|
||||
scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
|
||||
if (!attached) {
|
||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
|
||||
return;
|
||||
@ -311,9 +310,9 @@ void ComputeIK::compute() {
|
||||
|
||||
// validate placed link for collisions
|
||||
collision_detection::CollisionResult collisions;
|
||||
bool colliding = !ignore_collisions && isTargetPoseColliding(sandbox_scene, target_pose, link, &collisions);
|
||||
|
||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
||||
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
|
||||
bool colliding =
|
||||
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
|
||||
|
||||
// markers used for failures
|
||||
std::deque<visualization_msgs::Marker> failure_markers;
|
||||
@ -336,7 +335,9 @@ void ComputeIK::compute() {
|
||||
solution.markAsFailure();
|
||||
// TODO: visualize collisions
|
||||
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
||||
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||
auto colliding_scene{ scene->diff() };
|
||||
colliding_scene->setCurrentState(sandbox_state);
|
||||
spawn(InterfaceState(colliding_scene), std::move(solution));
|
||||
return;
|
||||
} else
|
||||
generateVisualMarkers(sandbox_state, appender, links_to_visualize);
|
||||
@ -349,12 +350,12 @@ void ComputeIK::compute() {
|
||||
compare_state.setToDefaultValues(jmg, compare_pose_name);
|
||||
compare_state.copyJointGroupPositions(jmg, compare_pose);
|
||||
} else
|
||||
sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
|
||||
scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
|
||||
|
||||
double min_solution_distance = props.get<double>("min_solution_distance");
|
||||
|
||||
IKSolutions ik_solutions;
|
||||
auto is_valid = [sandbox_scene, ignore_collisions, min_solution_distance,
|
||||
auto is_valid = [scene, ignore_collisions, min_solution_distance,
|
||||
&ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
|
||||
const double* joint_positions) {
|
||||
for (const auto& sol : ik_solutions) {
|
||||
@ -365,7 +366,7 @@ void ComputeIK::compute() {
|
||||
ik_solutions.emplace_back();
|
||||
state->copyJointGroupPositions(jmg, ik_solutions.back());
|
||||
|
||||
return ignore_collisions || !sandbox_scene->isStateColliding(*state, jmg->getName());
|
||||
return ignore_collisions || !scene->isStateColliding(*state, jmg->getName());
|
||||
};
|
||||
|
||||
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
|
||||
@ -388,7 +389,7 @@ void ComputeIK::compute() {
|
||||
// for all new solutions (successes and failures)
|
||||
for (size_t i = previous; i != ik_solutions.size(); ++i) {
|
||||
// create a new scene for each solution as they will have different robot states
|
||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||
planning_scene::PlanningScenePtr solution_scene = scene->diff();
|
||||
SubTrajectory solution;
|
||||
solution.setComment(s.comment());
|
||||
|
||||
@ -403,11 +404,11 @@ void ComputeIK::compute() {
|
||||
solution.markAsFailure();
|
||||
|
||||
// set scene's robot state
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
||||
robot_state.update();
|
||||
robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
|
||||
solution_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
||||
solution_state.update();
|
||||
|
||||
InterfaceState state(scene);
|
||||
InterfaceState state(solution_scene);
|
||||
forwardProperties(*s.start(), state);
|
||||
spawn(std::move(state), std::move(solution));
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user