bool PR2ArmKinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request, kinematics_msgs::GetPositionIK::Response &response) { if(!active_) { ROS_ERROR("IK service not active"); return true; } if(!checkIKService(request,response,ik_solver_info_)) return true; geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped; geometry_msgs::PoseStamped pose_msg_out; if(tf_ != NULL) { if(!convertPoseToRootFrame(pose_msg_in,pose_msg_out,root_name_, *tf_)) { response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return true; } } else { ROS_WARN_STREAM("No tf listener. Can't transform anything"); response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return true; } request.ik_request.pose_stamped = pose_msg_out; return getPositionIKHelper(request, response); }
bool ArmKinematicsConstraintAware::getPositionIK(kinematics_msgs::GetPositionIK::Request &request, kinematics_msgs::GetPositionIK::Response &response) { if(!isReady(response.error_code)) { if(request.ik_request.pose_stamped.header.frame_id != root_name_) { response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return true; } } if(!checkIKService(request,response,chain_info_)) return true; planning_models::KinematicState* state; if(collision_models_interface_->isPlanningSceneSet()) { state = new planning_models::KinematicState(*collision_models_interface_->getPlanningSceneState()); } else { state = new planning_models::KinematicState(collision_models_interface_->getKinematicModel()); state->setKinematicStateToDefault(); } planning_environment::setRobotStateAndComputeTransforms(request.ik_request.robot_state, *state); geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped; geometry_msgs::PoseStamped pose_msg_out; planning_environment::setRobotStateAndComputeTransforms(request.ik_request.robot_state, *state); if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), solver_->getBaseName(), pose_msg_in.header, pose_msg_in.pose, pose_msg_out)) { response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; delete state; return true; } ROS_DEBUG_STREAM("Pose is " << pose_msg_out.pose.position.x << " " << pose_msg_out.pose.position.y << " " << pose_msg_out.pose.position.z); bool ik_valid = solver_->getPositionIK(pose_msg_out.pose, state, response.solution.joint_state, response.error_code); if(ik_valid) { response.error_code.val = response.error_code.SUCCESS; } delete state; return ik_valid; }