bool KDLRobotModel::computeFastIK(const std::vector<double> &pose, const std::vector<double> &start, std::vector<double> &solution)
{
//pose: {x,y,z,r,p,y} or {x,y,z,qx,qy,qz,qw}
KDL::Frame frame_des;
frame_des.p.x(pose[0]);
frame_des.p.y(pose[1]);
frame_des.p.z(pose[2]);
// RPY
if(pose.size() == 6)
frame_des.M = KDL::Rotation::RPY(pose[3],pose[4],pose[5]);
// quaternion
else
frame_des.M = KDL::Rotation::Quaternion(pose[3],pose[4],pose[5],pose[6]);
// transform into kinematics frame
frame_des = T_planning_to_kinematics_ * frame_des;
// seed configuration
for(size_t i = 0; i < start.size(); i++)
jnt_pos_in_(i) = angles::normalize_angle(start[i]); // must be normalized for CartToJntSearch
if(ik_solver_->CartToJnt(jnt_pos_in_, frame_des, jnt_pos_out_) < 0)
return false;
solution.resize(start.size());
for(size_t i = 0; i < solution.size(); ++i)
solution[i] = angles::normalize_angle(jnt_pos_out_(i));
return true;
}
bool KDLRobotModel::computeIKSearch(const std::vector<double> &pose, const std::vector<double> &start, std::vector<double> &solution, double timeout)
{
//pose: {x,y,z,r,p,y} or {x,y,z,qx,qy,qz,qw}
KDL::Frame frame_des;
frame_des.p.x(pose[0]);
frame_des.p.y(pose[1]);
frame_des.p.z(pose[2]);
// RPY
if(pose.size() == 6)
frame_des.M = KDL::Rotation::RPY(pose[3],pose[4],pose[5]);
// quaternion
else
frame_des.M = KDL::Rotation::Quaternion(pose[3],pose[4],pose[5],pose[6]);
// transform into kinematics frame
frame_des = T_planning_to_kinematics_ * frame_des;
// seed configuration
for(size_t i = 0; i < start.size(); i++)
jnt_pos_in_(i) = angles::normalize_angle(start[i]); // must be normalized for CartToJntSearch
double initial_guess = jnt_pos_in_(free_angle_);
double search_discretization_angle = 0.02;
ros::Time start_time = ros::Time::now();
double loop_time = 0;
int count = 0;
int num_positive_increments = (int)((min_limits_[free_angle_]-initial_guess)/search_discretization_angle);
int num_negative_increments = (int)((initial_guess-min_limits_[free_angle_])/search_discretization_angle);
while(loop_time < timeout)
{
if(ik_solver_->CartToJnt(jnt_pos_in_, frame_des, jnt_pos_out_) >= 0)
{
solution.resize(start.size());
for(size_t i = 0; i < solution.size(); ++i)
solution[i] = angles::normalize_angle(jnt_pos_out_(i));
return true;
}
if(!getCount(count,num_positive_increments,-num_negative_increments))
return false;
jnt_pos_in_(free_angle_) = initial_guess + search_discretization_angle * count;
ROS_DEBUG("%d, %f",count,jnt_pos_in_(free_angle_));
loop_time = (ros::Time::now()-start_time).toSec();
}
if(loop_time >= timeout)
{
ROS_DEBUG("IK Timed out in %f seconds",timeout);
return false;
}
else
{
ROS_DEBUG("No IK solution was found");
return false;
}
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;
}
