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