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;
}
