diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index bef55a28ff..d94b951902 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -947,10 +947,9 @@ as the new values that correspond to the group */ set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the last link in the chain needs to achieve - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts = 0, + bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -959,11 +958,9 @@ as the new values that correspond to the group */ The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the \e tip link in the chain needs to achieve @param tip The name of the link the pose is specified for - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, - unsigned int attempts = 0, double timeout = 0.0, + bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -972,9 +969,8 @@ as the new values that correspond to the group */ The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the last link in the chain needs to achieve @param tip The name of the link the pose is specified for - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt */ - bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts = 0, + bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -982,11 +978,9 @@ as the new values that correspond to the group */ set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @param pose The pose the last link in the chain needs to achieve - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - unsigned int attempts = 0, double timeout = 0.0, + bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -996,11 +990,10 @@ as the new values that correspond to the group */ @param pose The pose the last link in the chain needs to achieve @param tip The name of the frame for which IK is attempted. @param consistency_limits This specifies the desired distance between the solution and the seed state - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - const std::vector& consistency_limits, unsigned int attempts = 0, double timeout = 0.0, + const std::vector& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -1011,11 +1004,10 @@ as the new values that correspond to the group */ to be in the same order as the order of the sub-groups in this group. Returns true on success. @param poses The poses the last link in each chain needs to achieve @param tips The names of the frames for which IK is attempted. - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, unsigned int attempts = 0, double timeout = 0.0, + const std::vector& tips, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -1027,12 +1019,10 @@ as the new values that correspond to the group */ @param poses The poses the last link in each chain needs to achieve @param tips The names of the frames for which IK is attempted. @param consistency_limits This specifies the desired distance between the solution and the seed state - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts = 0, double timeout = 0.0, + const std::vector& tips, const std::vector >& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); @@ -1042,12 +1032,11 @@ as the new values that correspond to the group */ @param poses The poses the last link in each chain needs to achieve @param tips The names of the frames for which IK is attempted. @param consistency_limits This specifies the desired distance between the solution and the seed state - @param attempts The number of times IK is attempted @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, - const std::vector >& consistency_limits, unsigned int attempts = 0, + const std::vector >& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 9bc3923343..2ecbf71b61 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1298,7 +1298,7 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig return true; } -bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, unsigned int attempts, +bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1308,20 +1308,20 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } - return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options); + return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, const std::string& tip, - unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint, + double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { Eigen::Isometry3d mat; tf2::fromMsg(pose, mat); static std::vector consistency_limits; - return setFromIK(jmg, mat, tip, consistency_limits, attempts, timeout, constraint, options); + return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint, options); } -bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, unsigned int attempts, +bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1332,15 +1332,15 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& return false; } static std::vector consistency_limits; - return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, attempts, timeout, constraint, options); + return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in, - unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint, + double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { static std::vector consistency_limits; - return setFromIK(jmg, pose_in, tip_in, consistency_limits, attempts, timeout, constraint, options); + return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint, options); } namespace @@ -1383,7 +1383,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& } bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in, - const std::vector& consistency_limits_in, unsigned int attempts, double timeout, + const std::vector& consistency_limits_in, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1397,21 +1397,21 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& std::vector > consistency_limits; consistency_limits.push_back(consistency_limits_in); - return setFromIK(jmg, poses, tips, consistency_limits, attempts, timeout, constraint, options); + return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in, - const std::vector& tips_in, unsigned int attempts, double timeout, + const std::vector& tips_in, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { - static const std::vector > CONSISTENCY_LIMITS; - return setFromIK(jmg, poses_in, tips_in, CONSISTENCY_LIMITS, attempts, timeout, constraint, options); + const std::vector > consistency_limits; + return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint, options); } bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in, const std::vector& tips_in, - const std::vector >& consistency_limit_sets, unsigned int attempts, + const std::vector >& consistency_limit_sets, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1615,9 +1615,6 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (timeout < std::numeric_limits::epsilon()) timeout = jmg->getDefaultIKTimeout(); - if (attempts == 0) - attempts = jmg->getDefaultIKAttempts(); - // set callback function kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) @@ -1626,47 +1623,17 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Bijection const std::vector& bij = jmg->getKinematicsSolverJointBijection(); - bool first_seed = true; std::vector initial_values; - for (unsigned int st = 0; st < attempts; ++st) - { - std::vector seed(bij.size()); - - // the first seed is the current robot state joint values - if (first_seed) - { - first_seed = false; copyJointGroupPositions(jmg, initial_values); + std::vector seed(bij.size()); for (std::size_t i = 0; i < bij.size(); ++i) seed[i] = initial_values[bij[i]]; - } - else - { - ROS_DEBUG_NAMED(LOGNAME, "Rerunning IK solver with random joint positions"); - - // sample a random seed - random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator(); - std::vector random_values; - jmg->getVariableRandomPositions(rng, random_values); - for (std::size_t i = 0; i < bij.size(); ++i) - seed[i] = random_values[bij[i]]; - - if (options.lock_redundant_joints) - { - std::vector red_joints; - solver->getRedundantJoints(red_joints); - copyJointGroupPositions(jmg, initial_values); - for (std::size_t i = 0; i < red_joints.size(); ++i) - seed[red_joints[i]] = initial_values[bij[red_joints[i]]]; - } - } // compute the IK solution std::vector ik_sol; moveit_msgs::MoveItErrorCodes error; - if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options, - this)) + if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options, this)) { std::vector solution(bij.size()); for (std::size_t i = 0; i < bij.size(); ++i) @@ -1674,13 +1641,12 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is setJointGroupPositions(jmg, solution); return true; } - } return false; } bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in, const std::vector& tips_in, - const std::vector >& consistency_limits, unsigned int attempts, + const std::vector >& consistency_limits, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) { @@ -1813,59 +1779,31 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: ik_queries[i].orientation.w = quat.w(); } - if (attempts == 0) - attempts = jmg->getDefaultIKAttempts(); - // if no timeout has been specified, use the default one if (timeout < std::numeric_limits::epsilon()) timeout = jmg->getDefaultIKTimeout(); - bool first_seed = true; - for (unsigned int st = 0; st < attempts; ++st) - { - bool found_solution = true; for (std::size_t sg = 0; sg < sub_groups.size(); ++sg) { const std::vector& bij = sub_groups[sg]->getKinematicsSolverJointBijection(); std::vector seed(bij.size()); - // the first seed is the initial state - if (first_seed) - { std::vector initial_values; copyJointGroupPositions(sub_groups[sg], initial_values); for (std::size_t i = 0; i < bij.size(); ++i) seed[i] = initial_values[bij[i]]; - } - else - { - // sample a random seed - random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator(); - std::vector random_values; - sub_groups[sg]->getVariableRandomPositions(rng, random_values); - for (std::size_t i = 0; i < bij.size(); ++i) - seed[i] = random_values[bij[i]]; - } // compute the IK solution std::vector ik_sol; moveit_msgs::MoveItErrorCodes error; const std::vector& climits = consistency_limits.empty() ? std::vector() : consistency_limits[sg]; - if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error)) - { + if (!solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error)) + return false; + std::vector solution(bij.size()); for (std::size_t i = 0; i < bij.size(); ++i) solution[bij[i]] = ik_sol[i]; setJointGroupPositions(sub_groups[sg], solution); - } - else - { - found_solution = false; - break; - } - ROS_DEBUG_NAMED(LOGNAME, "IK attempt: %d of %d", st, attempts); } - if (found_solution) - { std::vector full_solution; copyJointGroupPositions(jmg, full_solution); if (constraint ? constraint(this, jmg, &full_solution[0]) : true) @@ -1873,9 +1811,6 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: ROS_DEBUG_NAMED(LOGNAME, "Found IK solution"); return true; } - } - } - return false; } double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj,