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:
v4hn 2021-08-21 15:55:55 +02:00
parent 74d33c4ec0
commit ff86217f9a

View File

@ -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));
}