bool KDLArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state, const double &timeout, std::vector<double> &solution, const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback, const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback, int &error_code) { if(!active_) { ROS_ERROR("kinematics not active"); error_code = kinematics::INACTIVE; return false; } KDL::Frame pose_desired; tf::PoseMsgToKDL(ik_pose, pose_desired); //Do the IK KDL::JntArray jnt_pos_in; KDL::JntArray jnt_pos_out; jnt_pos_in.resize(dimension_); for(unsigned int i=0; i < dimension_; i++) jnt_pos_in(i) = ik_seed_state[i]; if(!desired_pose_callback.empty()) desired_pose_callback(ik_pose,ik_seed_state,error_code); if(error_code < 0) { ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision"); return false; } for(int i=0; i < max_search_iterations_; i++) { jnt_pos_in = getRandomConfiguration(); int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out); if(ik_valid < 0) continue; std::vector<double> solution_local; solution_local.resize(dimension_); for(unsigned int j=0; j < dimension_; j++) solution_local[j] = jnt_pos_out(j); solution_callback(ik_pose,solution_local,error_code); if(error_code == kinematics::SUCCESS) { solution = solution_local; return true; } } ROS_DEBUG("An IK that satisifes the constraints and is collision free could not be found"); if (error_code == 0) error_code = kinematics::NO_IK_SOLUTION; return false; }
bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state, double timeout, std::vector<double> &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const std::vector<double> &consistency_limits, const kinematics::KinematicsQueryOptions &options) const { ros::WallTime n1 = ros::WallTime::now(); if(!active_) { ROS_ERROR_NAMED("kdl","kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } if(ik_seed_state.size() != dimension_) { ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } if(!consistency_limits.empty() && consistency_limits.size() != dimension_) { ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } KDL::JntArray jnt_seed_state(dimension_); KDL::JntArray jnt_pos_in(dimension_); KDL::JntArray jnt_pos_out(dimension_); KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_); KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_); KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_); ik_solver_vel.setMimicJoints(mimic_joints_); ik_solver_pos.setMimicJoints(mimic_joints_); if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_)) { ROS_ERROR_NAMED("kdl","Could not set redundant joints"); return false; } if(options.lock_redundant_joints) { ik_solver_vel.lockRedundantJoints(); } solution.resize(dimension_); KDL::Frame pose_desired; tf::poseMsgToKDL(ik_pose, pose_desired); ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " " << ik_pose.orientation.z << " " << ik_pose.orientation.w); //Do the IK for(unsigned int i=0; i < dimension_; i++) jnt_seed_state(i) = ik_seed_state[i]; jnt_pos_in = jnt_seed_state; unsigned int counter(0); while(1) { // ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout); counter++; if(timedOut(n1,timeout)) { ROS_DEBUG_NAMED("kdl","IK timed out"); error_code.val = error_code.TIMED_OUT; ik_solver_vel.unlockRedundantJoints(); return false; } int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out); ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid); if(!consistency_limits.empty()) { getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints); if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out)) { ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits"); continue; } } else { getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints); ROS_DEBUG_NAMED("kdl","New random configuration"); for(unsigned int j=0; j < dimension_; j++) ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j)); if(ik_valid < 0 && !options.return_approximate_solution) { ROS_DEBUG_NAMED("kdl","Could not find IK solution"); continue; } } ROS_DEBUG_NAMED("kdl","Found IK solution"); for(unsigned int j=0; j < dimension_; j++) solution[j] = jnt_pos_out(j); if(!solution_callback.empty()) solution_callback(ik_pose,solution,error_code); else error_code.val = error_code.SUCCESS; if(error_code.val == error_code.SUCCESS) { ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations"); ik_solver_vel.unlockRedundantJoints(); return true; } } ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found"); error_code.val = error_code.NO_IK_SOLUTION; ik_solver_vel.unlockRedundantJoints(); return false; }
bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state, double timeout, std::vector<double> &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const std::vector<double> &consistency_limits) const { ros::WallTime n1 = ros::WallTime::now(); if(!active_) { ROS_ERROR("kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } if(ik_seed_state.size() != dimension_) { ROS_ERROR_STREAM("Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } if(!consistency_limits.empty() && consistency_limits.size() != dimension_) { ROS_ERROR_STREAM("Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } solution.resize(dimension_); KDL::Frame pose_desired; tf::poseMsgToKDL(ik_pose, pose_desired); ROS_DEBUG_STREAM("searchPositionIK2: Position request pose is " << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " " << ik_pose.orientation.z << " " << ik_pose.orientation.w); //Do the IK for(unsigned int i=0; i < dimension_; i++) jnt_seed_state_(i) = ik_seed_state[i]; jnt_pos_in_ = jnt_seed_state_; unsigned int counter(0); while(1) { // ROS_DEBUG("Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout); counter++; if(timedOut(n1,timeout)) { ROS_DEBUG("IK timed out"); error_code.val = error_code.TIMED_OUT; return false; } int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in_,pose_desired,jnt_pos_out_); if(!consistency_limits.empty()) { getRandomConfiguration(jnt_seed_state_, consistency_limits, jnt_pos_in_); if(ik_valid < 0 || !checkConsistency(jnt_seed_state_, consistency_limits, jnt_pos_out_)) { ROS_DEBUG("Could not find IK solution"); continue; } } else { getRandomConfiguration(jnt_pos_in_); if(ik_valid < 0) { ROS_DEBUG("Could not find IK solution"); continue; } } ROS_DEBUG("Found IK solution"); for(unsigned int j=0; j < dimension_; j++) solution[j] = jnt_pos_out_(j); if(!solution_callback.empty()) solution_callback(ik_pose,solution,error_code); else error_code.val = error_code.SUCCESS; if(error_code.val == error_code.SUCCESS) { ROS_DEBUG_STREAM("Solved after " << counter << " iterations"); return true; } } ROS_DEBUG("An IK that satisifes the constraints and is collision free could not be found"); error_code.val = error_code.NO_IK_SOLUTION; return false; }