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; }
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 SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses, const std::vector<double> &ik_seed_state, double timeout, const std::vector<double> &consistency_limits, std::vector<double> &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options) const { // Check if active if(!active_) { ROS_ERROR_NAMED("srv","kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Check if seed state correct if(ik_seed_state.size() != dimension_) { ROS_ERROR_STREAM_NAMED("srv","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Check that we have the same number of poses as tips if (tip_frames_.size() != ik_poses.size()) { ROS_ERROR_STREAM_NAMED("srv","Mismatched number of pose requests (" << ik_poses.size() << ") to tip frames (" << tip_frames_.size() << ") in searchPositionIK"); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Create the service message moveit_msgs::GetPositionIK ik_srv; ik_srv.request.ik_request.avoid_collisions = true; ik_srv.request.ik_request.group_name = getGroupName(); // Copy seed state into virtual robot state and convert into moveit_msg robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state); moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state); // Load the poses into the request in difference places depending if there is more than one or not geometry_msgs::PoseStamped ik_pose_st; ik_pose_st.header.frame_id = base_frame_; if (tip_frames_.size() > 1) { // Load into vector of poses for (std::size_t i = 0; i < tip_frames_.size(); ++i) { ik_pose_st.pose = ik_poses[i]; ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st); ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]); } } else { ik_pose_st.pose = ik_poses[0]; // Load into single pose value ik_srv.request.ik_request.pose_stamped = ik_pose_st; ik_srv.request.ik_request.ik_link_name = getTipFrames()[0]; } ROS_DEBUG_STREAM_NAMED("srv","Calling service: " << ik_service_client_->getService() ); if (ik_service_client_->call(ik_srv)) { // Check error code error_code.val = ik_srv.response.error_code.val; if(error_code.val != error_code.SUCCESS) { ROS_DEBUG_NAMED("srv","An IK that satisifes the constraints and is collision free could not be found."); switch (error_code.val) { // Debug mode for failure: ROS_DEBUG_STREAM("Request was: \n" << ik_srv.request.ik_request); ROS_DEBUG_STREAM("Response was: \n" << ik_srv.response.solution); case moveit_msgs::MoveItErrorCodes::FAILURE: ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: FAILURE"); break; case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION: ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: NO IK SOLUTION"); break; default: ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: " << error_code.val); } return false; } } else { ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService() ); error_code.val = error_code.FAILURE; return false; } // Convert the robot state message to our robot_state representation if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_)) { ROS_ERROR_STREAM_NAMED("srv","An error occured converting recieved robot state message into internal robot state."); error_code.val = error_code.FAILURE; return false; } // Get just the joints we are concerned about in our planning group robot_state_->copyJointGroupPositions(joint_model_group_, solution); // Run the solution callback (i.e. collision checker) if available if (!solution_callback.empty()) { ROS_DEBUG_STREAM_NAMED("srv","Calling solution callback on IK solution"); // hack: should use all poses, not just the 0th solution_callback(ik_poses[0], solution, error_code); if(error_code.val != error_code.SUCCESS) { switch (error_code.val) { case moveit_msgs::MoveItErrorCodes::FAILURE: ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: FAILURE"); break; case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION: ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: NO IK SOLUTION"); break; default: ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: " << error_code.val); } return false; } } ROS_INFO_STREAM_NAMED("srv","IK Solver Succeeded!"); return true; }