sbpl_arm_planner: orientation_solver.cpp Source File

00001 /*
00002  * Written by Gokul Subramanian, MSE in Robotics & CIS 2011
00003  * Advised by Professor Maxim Likhachev
00004  * University of Pennsylvania
00005  * 4/14/2010
00006 */
00007 
00008 #include <sbpl_arm_planner/pr2/sbpl_math.h>
00009 #include <sbpl_arm_planner/pr2/orientation_solver.h>
00010 #define PI 3.14159
00011 
00012 #define DEBUG 0
00013 #define END_EFF 9
00014 #define FOREARM_ROLL 6
00015 
00016 namespace sbpl_arm_planner
00017 {
00018 
00019 /*Both these pitch limits are on the upper side (i.e. if forearm is aligned along x-axis with forearm_roll=0, then the end effector cannot pitch upward more than WRIST_PITCH_LIMIT_MAX and cannot come below a pitch of WRIST_PITCH_LIMIT_MIN on the upper side. */
00020 #define WRIST_PITCH_LIMIT_MAX   114   //These values are in degrees (for ease of understanding).
00021 #define WRIST_PITCH_LIMIT_MIN   0     //The corresponding radian values are approximately 2 and 0.1.
00022 
00023 RPYSolver::RPYSolver(SBPLArmModel* arm, SBPLCollisionSpace* cspace)
00024 {
00025   arm_ = arm;
00026   cspace_ = cspace;
00027 
00028   verbose_ = false;
00029   try_both_directions_ = false;
00030   num_calls_ = 0;
00031   num_invalid_predictions_ = 0;
00032   num_invalid_solution_ = 0;
00033   num_invalid_path_to_solution_ = 0;
00034 }
00035 
00036 void RPYSolver::orientationSolver(double* output, double phi, double theta, double psi, double yaw1, double pitch1, double roll1, double yaw2, double pitch2, double roll2, int attempt) 
00037 {
00038   /**************************************************************************/
00039   //Is the desired orientation possible to attain with the given joint limits?
00040   /**************************************************************************/
00041   double wrist_pitch_limit_max;         //Used to express the wrist pitch limit in radians
00042   double wrist_pitch_limit_min;         //Used to express the wrist pitch limit in radians
00043   //The wrist pitch limit in radians
00044   wrist_pitch_limit_max=(PI/180.0)*WRIST_PITCH_LIMIT_MAX;
00045   wrist_pitch_limit_min=(PI/180.0)*WRIST_PITCH_LIMIT_MIN;
00046 
00047   //The PR2 FK is defined such that pitch is negative upward. Hence, the input pitch values are negated, because the following portion of the code was written assuming that pitch is positive upward.
00048   theta=-theta;
00049   pitch1=-pitch1;
00050   pitch2=-pitch2;
00051 
00052   double v1[] = {cos(theta)*cos(phi), cos(theta)*sin(phi), sin(theta)};
00053   double v2[] = {cos(pitch2)*cos(yaw2), cos(pitch2)*sin(yaw2), sin(pitch2)};
00054   double dp_v = dot_product(v1, v2, 3);
00055   double check_flag=dp_v<0;
00056   double check_flag2=fabs(dp_v)>fabs(cos(wrist_pitch_limit_max));
00057   double check_flag3=dp_v>0;
00058   double check_flag4=dp_v>cos(wrist_pitch_limit_min);
00059 
00060   if((check_flag && check_flag2) || (check_flag3 && check_flag4)) {
00061     //  std::cout << "Impossible." << std::endl;
00062     output[0]=0;
00063     output[1]=0;
00064     output[2]=0;
00065     output[3]=0;
00066     return;
00067   }
00068 
00069   /*****************************************************/
00070   //Firstly, all variables are declared and some defined
00071   /*****************************************************/
00072 
00073   double hand1[]={0, -0.5, 0, 0, 0.5, 0};
00074   double hand2[]={0, -0.5, 0, 0, 0.5, 0};
00075   double hand1_fo[6];
00076   double hand2_fo[6];
00077   double hand1_vect[3], hand2_vect[3];
00078   double grip1[]={0, 0, 0, 1, 0, 0};
00079   double grip2[]={0, 0, 0, 1, 0, 0};
00080   double grip1_fo[6];
00081   double grip2_fo[6];
00082   double grip1_vect[3], grip2_vect[3];
00083   double indicator1[]={-2.5, 0, 0, -2.5, 0, 1};
00084   double indicator2[]={-2.5, 0, 0, -2.5, 0, 1};
00085   double indicator1_fo[6];
00086   double indicator2_fo[6];
00087   double ind1_vect[3],ind2_vect[3];
00088   double comp1_vect[3], comp2_vect[3], project1_vect[3], project2_vect[3];
00089   double rot1[9], rot2[9], rot3[9];
00090   double temp[6], temp1[9], temp2[9], temp3[9], temp_var, temp_vect[3], temp2_vect[3];
00091   double w[3], w_hat[9];
00092   double rot_world[9], rot_world_trans[9];
00093   double identity[]={1, 0, 0,
00094                      0, 1, 0,
00095                      0, 0, 1};
00096   double unit_x[3]={1, 0, 0};
00097   double zero_vect[3]={0, 0, 0};
00098   double fo_arm_roll, wrist_pitch, wrist_roll, fs_angle, fe_angle;
00099   double is_orient_possible_flag=1;     //Flag returned indicates whether desired orientation is possible
00100   //with wrist limits. 1-possible, 0-impossible
00101   double c_alpha, c_beta, c_gamma, c_delta, c_eps;
00102 
00103   /******************************/
00104   //Accepting the input arguments
00105   /******************************/
00106 
00107   create_rotation_matrix(rot_world,phi,theta,psi);
00108   transpose(rot_world_trans, rot_world, 3, 3);
00109 
00110   //The start and the end orientations in the world frame
00111   rot1[0]=cos(yaw1);      rot1[3]=-sin(yaw1);     rot1[6]=0;
00112   rot1[1]=sin(yaw1);      rot1[4]=cos(yaw1);      rot1[7]=0;
00113   rot1[2]=0;              rot1[5]=0;              rot1[8]=1;
00114 
00115   rot2[0]=cos(pitch1);    rot2[3]=0;              rot2[6]=-sin(pitch1);
00116   rot2[1]=0;              rot2[4]=1;              rot2[7]=0;
00117   rot2[2]=sin(pitch1);    rot2[5]=0;              rot2[8]=cos(pitch1);
00118 
00119   multiply(rot3,rot1,3,3,rot2,3); //Yaw and pitch rotations
00120   multiply(temp,rot3,3,3,hand1,2);
00121   equate(hand1,temp,3,2);
00122   multiply(temp,rot3,3,3,grip1,2);
00123   equate(grip1,temp,3,2);
00124 
00125   w[0]=grip1[3]; //Unit vector along grip
00126   w[1]=grip1[4];
00127   w[2]=grip1[5];
00128 
00129   w_hat[0]=0;         w_hat[3]=-w[2];     w_hat[6]=w[1];
00130   w_hat[1]=w[2];      w_hat[4]=0;         w_hat[7]=-w[0];
00131   w_hat[2]=-w[1];     w_hat[5]=w[0];      w_hat[8]=0;
00132 
00133   scalar_multiply(temp1,w_hat,3,3,sin(roll1));
00134   multiply(temp2,w_hat,3,3,w_hat,3);
00135   scalar_multiply(temp3,temp2,3,3,1-cos(roll1));
00136   matrix_add(temp2,temp1,temp3,3,3);
00137   matrix_add(rot1,identity, temp2,3,3);
00138 
00139   multiply(temp1,rot1,3,3,hand1,2);
00140   equate(hand1,temp1,3,2);
00141 
00142   rot1[0]=cos(yaw2);      rot1[3]=-sin(yaw2);     rot1[6]=0;
00143   rot1[1]=sin(yaw2);      rot1[4]=cos(yaw2);      rot1[7]=0;
00144   rot1[2]=0;              rot1[5]=0;              rot1[8]=1;
00145 
00146   rot2[0]=cos(pitch2);    rot2[3]=0;              rot2[6]=-sin(pitch2);
00147   rot2[1]=0;              rot2[4]=1;              rot2[7]=0;
00148   rot2[2]=sin(pitch2);    rot2[5]=0;              rot2[8]=cos(pitch2);
00149 
00150   multiply(rot3,rot1,3,3,rot2,3); //Yaw and pitch rotations
00151   multiply(temp,rot3,3,3,hand2,2);
00152   equate(hand2,temp,3,2);
00153   multiply(temp,rot3,3,3,grip2,2);
00154   equate(grip2,temp,3,2);
00155 
00156   w[0]=grip2[3]; //Unit vector along grip
00157   w[1]=grip2[4];
00158   w[2]=grip2[5];
00159 
00160   w_hat[0]=0;         w_hat[3]=-w[2];     w_hat[6]=w[1];
00161   w_hat[1]=w[2];      w_hat[4]=0;         w_hat[7]=-w[0];
00162   w_hat[2]=-w[1];     w_hat[5]=w[0];      w_hat[8]=0;
00163 
00164   scalar_multiply(temp1,w_hat,3,3,sin(roll2));
00165   multiply(temp2,w_hat,3,3,w_hat,3);
00166   scalar_multiply(temp3,temp2,3,3,1-cos(roll2));
00167   matrix_add(temp2,temp1,temp3,3,3);
00168   matrix_add(rot1,identity,temp2,3,3);
00169 
00170   multiply(temp1,rot1,3,3,hand2,2);
00171   equate(hand2,temp1,3,2);
00172 
00173   //The start and the end orientations in the forearm frame
00174   multiply(hand1_fo,rot_world_trans,3,3,hand1,2);
00175   multiply(hand2_fo,rot_world_trans,3,3,hand2,2);
00176   multiply(grip1_fo,rot_world_trans,3,3,grip1,2);
00177   multiply(grip2_fo,rot_world_trans,3,3,grip2,2);
00178 
00179   grip1_vect[0]=grip1_fo[3];
00180   grip1_vect[1]=grip1_fo[4];
00181   grip1_vect[2]=grip1_fo[5];
00182 
00183   temp_var=dot_product(grip1_vect, unit_x, 3);
00184   scalar_multiply(comp1_vect, unit_x,3,1,temp_var);
00185   subtract(project1_vect,grip1_vect,comp1_vect,3,1);
00186 
00187   grip2_vect[0]=grip2_fo[3];
00188   grip2_vect[1]=grip2_fo[4];
00189   grip2_vect[2]=grip2_fo[5];
00190 
00191   temp_var=dot_product(grip2_vect, unit_x, 3);
00192   scalar_multiply(comp2_vect, unit_x,3,1,temp_var);
00193   subtract(project2_vect,grip2_vect,comp2_vect,3,1);
00194 
00195   ind1_vect[0]=indicator1[3]-indicator1[0];
00196   ind1_vect[1]=indicator1[4]-indicator1[1];
00197   ind1_vect[2]=indicator1[5]-indicator1[2];
00198 
00199   ind2_vect[0]=indicator2[3]-indicator2[0];
00200   ind2_vect[1]=indicator2[4]-indicator2[1];
00201   ind2_vect[2]=indicator2[5]-indicator2[2];
00202 
00203   if(!check_equality(grip1_vect, comp1_vect, 3)) {
00204     c_delta=dot_product(ind1_vect, project1_vect, 3)/vect_norm(project1_vect,3);
00205     fs_angle=acos(c_delta);
00206 
00207     cross_product(temp_vect, ind1_vect, project1_vect);
00208 
00209     if(vect_divide(temp_vect, unit_x, 3)<0) {
00210       fs_angle=-fs_angle;
00211     }
00212 
00213     rot1[0]=1;              rot1[3]=0;               rot1[6]=0;
00214     rot1[1]=0;              rot1[4]=cos(fs_angle);   rot1[7]=-sin(fs_angle);
00215     rot1[2]=0;              rot1[5]=sin(fs_angle);   rot1[8]=cos(fs_angle);
00216 
00217     multiply(temp, rot1, 3,3, indicator1, 2);
00218     equate(indicator1_fo, temp, 3, 2);
00219   }
00220   else {
00221     equate(indicator1_fo, indicator1, 3,2);
00222   }
00223 
00224   if(!check_equality(grip2_vect, comp2_vect, 3)) {
00225     c_eps=dot_product(ind2_vect, project2_vect, 3)/vect_norm(project2_vect, 3);
00226     fe_angle=acos(c_eps);
00227 
00228     cross_product(temp_vect, ind2_vect, project2_vect);
00229 
00230     if(vect_divide(temp_vect, unit_x, 3)<0) {
00231       fe_angle=-fe_angle;
00232     }
00233 
00234     rot1[0]=1;              rot1[3]=0;               rot1[6]=0;
00235     rot1[1]=0;              rot1[4]=cos(fe_angle);   rot1[7]=-sin(fe_angle);
00236     rot1[2]=0;              rot1[5]=sin(fe_angle);   rot1[8]=cos(fe_angle);
00237 
00238     multiply(temp, rot1, 3,3, indicator2, 2);
00239     equate(indicator2_fo, temp, 3, 2);
00240   }
00241   else {
00242     equate(indicator2_fo, indicator2, 3,2);
00243   }
00244 
00245   /*********************************/
00246   //Forearm roll is calculated now
00247   /*********************************/
00248 
00249   ind1_vect[0]=indicator1_fo[3]-indicator1_fo[0];
00250   ind1_vect[1]=indicator1_fo[4]-indicator1_fo[1];
00251   ind1_vect[2]=indicator1_fo[5]-indicator1_fo[2];
00252 
00253   ind2_vect[0]=indicator2_fo[3]-indicator2_fo[0];
00254   ind2_vect[1]=indicator2_fo[4]-indicator2_fo[1];
00255   ind2_vect[2]=indicator2_fo[5]-indicator2_fo[2];
00256 
00257   c_gamma=dot_product(ind1_vect, ind2_vect, 3);
00258 
00259   cross_product(temp_vect, ind1_vect, ind2_vect);
00260   if(vect_divide(temp_vect, unit_x, 3)>0) {
00261     fo_arm_roll=acos(c_gamma);
00262   }       
00263   else {
00264     fo_arm_roll=-acos(c_gamma);
00265   } 
00266 
00267   //In the second attempt try rotating the other way around to the same orientation
00268   if(attempt == 2) {fo_arm_roll = -(2*PI - fo_arm_roll);}
00269 
00270   rot1[0]=1;              rot1[3]=0;                  rot1[6]=0;
00271   rot1[1]=0;              rot1[4]=cos(fo_arm_roll);   rot1[7]=-sin(fo_arm_roll);
00272   rot1[2]=0;              rot1[5]=sin(fo_arm_roll);   rot1[8]=cos(fo_arm_roll);
00273 
00274   multiply(temp1,rot1,3,3,hand1_fo,2);
00275   equate(hand1_fo,temp1,3,2);
00276   multiply(temp1,rot1,3,3,grip1_fo,2);
00277   equate(grip1_fo,temp1,3,2);
00278 
00279   /*********************************/
00280   //Wrist pitch is calculated now
00281   /*********************************/
00282 
00283   grip1_vect[0]=grip1_fo[3];
00284   grip1_vect[1]=grip1_fo[4];
00285   grip1_vect[2]=grip1_fo[5];
00286 
00287   grip2_vect[0]=grip2_fo[3];
00288   grip2_vect[1]=grip2_fo[4];
00289   grip2_vect[2]=grip2_fo[5];
00290 
00291   cross_product(temp_vect, grip1_vect, grip2_vect);
00292 
00293   if(!check_equality(temp_vect, zero_vect, 3)) {
00294     c_alpha=dot_product(grip1_vect, grip2_vect, 3);
00295 
00296     cross_product(temp2_vect,ind2_vect,temp_vect);
00297 
00298     if(vect_divide(temp2_vect, unit_x, 3)>0) {
00299       wrist_pitch=-acos(c_alpha);
00300       temp_vect[0]=-temp_vect[0];
00301       temp_vect[1]=-temp_vect[1];
00302       temp_vect[2]=-temp_vect[2];
00303     }
00304     else {
00305       wrist_pitch=acos(c_alpha);
00306 
00307     }        
00308 
00309     scalar_multiply(w, temp_vect, 3,1,1/vect_norm(temp_vect, 3));
00310 
00311     w_hat[0]=0;         w_hat[3]=-w[2];     w_hat[6]=w[1];
00312     w_hat[1]=w[2];      w_hat[4]=0;         w_hat[7]=-w[0];
00313     w_hat[2]=-w[1];     w_hat[5]=w[0];      w_hat[8]=0;
00314 
00315     scalar_multiply(temp1,w_hat,3,3,sin(wrist_pitch));
00316     multiply(temp2,w_hat,3,3,w_hat,3);
00317     scalar_multiply(temp3,temp2,3,3,1-cos(wrist_pitch));
00318     matrix_add(temp2,temp1,temp3,3,3);
00319     matrix_add(rot1,identity, temp2,3,3);
00320 
00321     multiply(temp1,rot1,3,3,hand1_fo,2);
00322     equate(hand1_fo,temp1,3,2);
00323     multiply(temp1,rot1,3,3,grip1_fo,2);
00324     equate(grip1_fo,temp1,3,2);
00325   }
00326   else {
00327     wrist_pitch=0;
00328   }
00329 
00332   /*********************************/
00333   //Wrist roll is calculated now
00334   /*********************************/
00335   wrist_roll=0;
00336   hand1_vect[0]=hand1_fo[3]-hand1_fo[0];
00337   hand1_vect[1]=hand1_fo[4]-hand1_fo[1];
00338   hand1_vect[2]=hand1_fo[5]-hand1_fo[2];
00339 
00340   hand2_vect[0]=hand2_fo[3]-hand2_fo[0];
00341   hand2_vect[1]=hand2_fo[4]-hand2_fo[1];
00342   hand2_vect[2]=hand2_fo[5]-hand2_fo[2];
00343 
00344   grip1_vect[0]=grip1_fo[3];
00345   grip1_vect[1]=grip1_fo[4];
00346   grip1_vect[2]=grip1_fo[5];
00347 
00348   c_beta=dot_product(hand1_vect, hand2_vect, 3);
00349   cross_product(temp_vect, hand1_vect, hand2_vect);
00350 
00351   if(!check_equality(temp_vect, zero_vect, 3)) {
00352     if(vect_divide(temp_vect, grip1_vect, 3)>0) {
00353       wrist_roll=acos(c_beta);
00354     }
00355     else {
00356       wrist_roll=-acos(c_beta);
00357     }
00358   }
00359 
00360   //The output
00361   output[0]=is_orient_possible_flag;
00362   output[1]=fo_arm_roll;   
00363   output[2]=wrist_pitch;
00364   output[3]=wrist_roll;
00365 }
00366 
00367 bool RPYSolver::isOrientationFeasible(const double* rpy, std::vector<double> &start, std::vector<double> &prefinal, std::vector<double> &final)
00368 {
00369   bool try_both_rotations = try_both_directions_;
00370   unsigned char dist=0;
00371   unsigned int i=0;
00372   unsigned int my_count=0;
00373   double hand_rotations[4];
00374   std::vector<double>forearm_pose(6,0);
00375   std::vector<double>endeff_pose(6,0);
00376   std::vector<double>goal_joint_config(7,0);
00377 
00378   int link_num = 2; //added 6/15/11
00379 
00380   //get pose of forearm link
00381   if(!arm_->computeFK(start,FOREARM_ROLL,forearm_pose))
00382   {
00383     SBPL_DEBUG("computeFK failed on forearm pose.\n");
00384     return false;
00385   }
00386 
00387   //get pose of end effector link
00388   if(!arm_->computeFK(start,END_EFF,endeff_pose))
00389   {
00390     SBPL_DEBUG("computeFK failed on end_eff pose.\n");
00391     return false;
00392   }
00393 
00394 #if DEBUG
00395   SBPL_DEBUG_NAMED("orientation_solver", "Joint Angles: %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f", start[0], start[1], start[2], start[3], start[4], start[5], start[6]);
00396   SBPL_DEBUG_NAMED("orientation_solver", "FK:forearm: %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f",forearm_pose[0], forearm_pose[1], forearm_pose[2], forearm_pose[3], forearm_pose[4], forearm_pose[5], forearm_pose[6]);
00397   SBPL_DEBUG_NAMED("orientation_solver", "FK:endeff:  %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f",endeff_pose[0], endeff_pose[1], endeff_pose[2], endeff_pose[3], endeff_pose[4], endeff_pose[5], endeff_pose[6]);
00398   SBPL_DEBUG_NAMED("orientation_solver", "Start yaw: %0.4f, Start pitch: %0.4f, Start roll: %0.4f",endeff_pose[5], endeff_pose[4], endeff_pose[3]);
00399   SBPL_DEBUG_NAMED("orientation_solver", "End yaw required: %0.4f, End pitch required: %0.4f, End roll required: %0.4f", rpy[2], rpy[1], rpy[0]);
00400 #endif
00401 
00402   // call orientation planner 
00403   orientationSolver(hand_rotations, forearm_pose[5],forearm_pose[4],forearm_pose[3], endeff_pose[5], endeff_pose[4], endeff_pose[3], rpy[2], rpy[1], rpy[0], 1); //1st attempt
00404 
00405   num_calls_++;
00406 
00407 #if DEBUG
00408   SBPL_DEBUG_NAMED("orientation_solver", "[isGoalPositionGokul] The rotations commanded in the first attempt are: %0.4f, %0.4f and %0.4f", hand_rotations[1], hand_rotations[2], hand_rotations[3]);
00409 #endif
00410 
00411   if(hand_rotations[0]==0)
00412   {
00413 #if DEBUG
00414     SBPL_DEBUG_NAMED("orientation_solver", "Orientation solver predicts that this movement is impossible with the given pitch limits.");
00415 #endif
00416     num_invalid_predictions_++;
00417     return false;
00418   }
00419 
00420 SET_ANGLES_AGAIN: //GoTO label, temporary, do not continue use of this
00421 
00422   for(my_count=0; my_count<start.size(); my_count++)
00423   {
00424     goal_joint_config[my_count]=start[my_count];
00425     prefinal[my_count]=start[my_count];
00426   }
00427 
00428   for(my_count=0; my_count<3; my_count++)
00429   {
00430     goal_joint_config[4+my_count]+=hand_rotations[1+my_count];
00431 
00432     //check for collisions
00433     //if(!cspace_->checkCollision(goal_joint_config, verbose_, false, dist))
00434     if(!cspace_->checkLinkForCollision(goal_joint_config, link_num, verbose_, dist))
00435     {
00436       num_invalid_solution_++;
00437       return false;
00438     }
00439     std::vector<std::vector<double> > path(2, std::vector<double>(arm_->num_joints_,0));
00440 
00441     for(i = 0; i < path[0].size(); i++)
00442     {
00443       path[0][i] = start[i];
00444       path[1][i] = goal_joint_config[i];
00445     }
00446 
00447     //check for collisions along the path
00448     if(!cspace_->checkLinkPathForCollision(start, goal_joint_config, link_num, verbose_, dist))
00449     {
00450       //try rotating in the opposite direction
00451       if(try_both_rotations)
00452       {
00453         orientationSolver(hand_rotations, forearm_pose[5],forearm_pose[4],forearm_pose[3], endeff_pose[5], endeff_pose[4], endeff_pose[3], rpy[2], rpy[1], rpy[0], 2);
00454         try_both_rotations = 0;
00455         goto SET_ANGLES_AGAIN;
00456       }
00457 
00458       num_invalid_path_to_solution_++;
00459       return false;
00460     }
00461   }
00462 
00463   final = goal_joint_config;
00464 
00465 #if DEBUG
00466   SBPL_DEBUG_NAMED("orientation_solver", "\n \n After:");
00467   arm_->computeFK(final,END_EFF,endeff_pose);
00468   SBPL_DEBUG_NAMED("orientation_solver", "The perfect roll value from FK is: %0.4f.", rpy[0]-endeff_pose[3]);
00469 
00470   SBPL_DEBUG_NAMED("orientation_solver", "Joint angles:%0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f", final[0], final[1], final[2], final[3], final[4], final[5], final[6]);
00471 
00472   SBPL_DEBUG_NAMED("orientation_solver", "Forward kinematics on final arm position.");
00473   arm_->computeFK(final,FOREARM_ROLL,forearm_pose);
00474   SBPL_DEBUG_NAMED("orientation_solver", "FK:f_arm: %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f",forearm_pose[0], forearm_pose[1], forearm_pose[2], forearm_pose[3], forearm_pose[4], forearm_pose[5], forearm_pose[6]);
00475   arm_->computeFK(final,END_EFF,endeff_pose);
00476   SBPL_DEBUG_NAMED("orientation_solver", "FK:endeff: %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f, %0.4f",endeff_pose[0], endeff_pose[1], endeff_pose[2], endeff_pose[3], endeff_pose[4], endeff_pose[5], endeff_pose[6]);
00477 
00478   SBPL_DEBUG_NAMED("orientation_solver", "Succeeded.");
00479 #endif
00480 
00481   return true;
00482 }
00483 
00484 
00485 void RPYSolver::printStats()
00486 {
00487   SBPL_INFO("Calls to OS: %d   Predicts Impossible: %d   Invalid Solutions: %d   Invalid Paths: %d", num_calls_, num_invalid_predictions_, num_invalid_solution_, num_invalid_path_to_solution_); 
00488 }
00489 
00490 }
00491