From ee3356821e330c78dbb9055ebd17b0898ae7b7cf Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 20 Feb 2019 20:33:41 -0700 Subject: [PATCH] Remove attempts from setFromIK --- src/imarker_end_effector.cpp | 3 +-- src/imarker_robot_state.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/imarker_end_effector.cpp b/src/imarker_end_effector.cpp index 1afc514..093b591 100644 --- a/src/imarker_end_effector.cpp +++ b/src/imarker_end_effector.cpp @@ -136,7 +136,6 @@ void IMarkerEndEffector::iMarkerCallback(const visualization_msgs::InteractiveMa void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose) { // Cartesian settings - const std::size_t attempts = 2; const double timeout = 1.0 / 30.0; // 30 fps // Optionally collision check @@ -151,7 +150,7 @@ void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose) } // Attempt to set robot to new pose - if (imarker_state_->setFromIK(arm_data_.jmg_, pose, arm_data_.ee_link_->getName(), attempts, timeout, constraint_fn)) + if (imarker_state_->setFromIK(arm_data_.jmg_, pose, arm_data_.ee_link_->getName(), timeout, constraint_fn)) { imarker_state_->update(); // if (psm_->getPlanningScene()->isStateValid(*imarker_state_)) diff --git a/src/imarker_robot_state.cpp b/src/imarker_robot_state.cpp index 114978e..89bc6fb 100644 --- a/src/imarker_robot_state.cpp +++ b/src/imarker_robot_state.cpp @@ -281,7 +281,6 @@ bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses, // ROS_DEBUG_STREAM_NAMED(name_, "First pose should be for joint model group: " << arm_datas_[0].ee_link_->getName()); - const std::size_t attempts = 10; const double timeout = 1.0 / 30.0; // 30 fps // Optionally collision check @@ -303,7 +302,7 @@ bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses, std::size_t outer_attempts = 20; for (std::size_t i = 0; i < outer_attempts; ++i) { - if (!imarker_state_->setFromIK(group, poses, tips, attempts, timeout, constraint_fn)) + if (!imarker_state_->setFromIK(group, poses, tips, timeout, constraint_fn)) { ROS_DEBUG_STREAM_NAMED(name_, "Failed to find dual arm pose, trying again");