int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
const KDL::Frame& p_in,
KDL::JntArray &q_out,
const double &timeout)
{
KDL::JntArray q_init = q_in;
double initial_guess = q_init(free_angle_);
ros::Time start_time = ros::Time::now();
double loop_time = 0;
int count = 0;
int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
while(loop_time < timeout)
{
if(CartToJnt(q_init,p_in,q_out) > 0)
return 1;
if(!getCount(count,num_positive_increments,-num_negative_increments))
return -1;
q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
ROS_DEBUG("%d, %f",count,q_init(free_angle_));
loop_time = (ros::Time::now()-start_time).toSec();
}
if(loop_time >= timeout)
{
ROS_DEBUG("IK Timed out in %f seconds",timeout);
return TIMED_OUT;
}
else
{
ROS_DEBUG("No IK solution was found");
return NO_IK_SOLUTION;
}
return NO_IK_SOLUTION;
}
int TreeIdSolver_RNE::CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
{
Wrench dummy;
return CartToJnt(q,q_dot,q_dotdot,Twist::Zero(),ag,f_ext,torques,dummy);
}
