compute ik: ignore collisions skips early eef check

This commit is contained in:
v4hn 2018-05-30 16:15:12 +02:00
parent 4026661bfe
commit 0091c76208

View File

@ -215,6 +215,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
properties().performInitFrom(INTERFACE, s.start()->properties(), true); properties().performInitFrom(INTERFACE, s.start()->properties(), true);
const auto& props = properties(); const auto& props = properties();
const bool ignore_collisions = props.get<bool>("ignore_collisions");
const auto& robot_model = sandbox_scene->getRobotModel(); const auto& robot_model = sandbox_scene->getRobotModel();
const moveit::core::JointModelGroup* eef_jmg = nullptr; const moveit::core::JointModelGroup* eef_jmg = nullptr;
const moveit::core::JointModelGroup* jmg = nullptr; const moveit::core::JointModelGroup* jmg = nullptr;
@ -276,7 +277,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
// validate placed link for collisions // validate placed link for collisions
collision_detection::CollisionResult collisions; collision_detection::CollisionResult collisions;
bool colliding = isTargetPoseColliding(sandbox_scene, target_pose, link, &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 = sandbox_scene->getCurrentStateNonConst();
@ -317,7 +318,6 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose); sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
IKSolutions ik_solutions; IKSolutions ik_solutions;
bool ignore_collisions = props.get<bool>("ignore_collisions");
auto isValid = [sandbox_scene, ignore_collisions, &ik_solutions] auto isValid = [sandbox_scene, ignore_collisions, &ik_solutions]
(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) { (robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) {
for (const auto& sol : ik_solutions){ for (const auto& sol : ik_solutions){