SimpleGraspPlanner.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009-2010, U. Klank
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *
00014  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00015  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00016  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00017  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00018  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00019  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00020  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00021  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00022  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00023  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00024  * POSSIBILITY OF SUCH DAMAGE.
00025  *
00026  */
00027 
00028 #include "SimpleGraspPlanner.h"
00029 #include <math.h>
00030 #include <stdio.h>
00031 
00032 
00033 typedef struct
00034 {
00035   Point3D* obstacle_list;
00036   CovariancePoint* obstacle_covs;
00037   int num_obstacles;
00038   Point3D* hand_points;
00039   int num_hand_points;  double max_finger;
00040   double palm_width;
00041   double table_z;
00042 }
00043 Dataset;
00044 
00045 Dataset g_data;
00046 #define min(A,B) (((A) < (B)) ? (A) : (B))
00047 #define max(A,B) (((A) > (B)) ? (A) : (B))
00048 
00052 int InitSGP(Point3D* hand_points, int num, double table_z)
00053 {
00054   int iter_hand_point;
00055   g_data.hand_points = hand_points;
00056   g_data.num_hand_points = num;
00057   g_data.max_finger = 0.0;
00058   g_data.palm_width = 0.0;
00059 
00060   printf("Hand Points:\n");
00061   for(iter_hand_point = 0; iter_hand_point < num; iter_hand_point++)
00062   {
00064     if(g_data.max_finger < hand_points[iter_hand_point].z)
00065       g_data.max_finger = hand_points[iter_hand_point].z;
00067     if(g_data.palm_width < fabs(hand_points[iter_hand_point].y))
00068       g_data.palm_width = fabs(hand_points[iter_hand_point].y);
00069     printf("%f %f %f\n", hand_points[iter_hand_point].x, hand_points[iter_hand_point].y, hand_points[iter_hand_point].z);
00070   }
00071   g_data.table_z = table_z;
00072   /*printf("Max Finger Length: %f Palm Width %f Table %f\n", g_data.max_finger, g_data.palm_width,  g_data.table_z);*/
00073 
00074   return num;
00075 }
00076 
00077 void setTableHeight(double table_z)
00078 {
00079   g_data.table_z = table_z;
00080 }
00081 
00082 
00083 double getTableHeight()
00084 {
00085   return (g_data.table_z);
00086 }
00087 
00088 
00099 double det(CovariancePoint cov)
00100 {
00101   return cov.sx * cov.sy * cov.sz -
00102          cov.sx * cov.szy * cov.syz +
00103          cov.sxy * cov.szy * cov.szx -
00104          cov.sxy * cov.syx * cov.sz +
00105          cov.sxz * cov.syx * cov.syz -
00106          cov.sxz * cov.sy * cov.szx;
00107 }
00143 double f(Point3D point_test, Point3D mean, CovariancePoint cov)
00144 {
00145   double c_temp, expo, det_temp;
00146   double dx =  mean.x - point_test.x;
00147   double dy =  mean.y - point_test.y;
00148   double dz =  mean.z - point_test.z;
00149   double cov_bla = cov.szx * cov.sxy * cov.syz - cov.szx * cov.sxz *  cov.sy - cov.syx * cov.sxy * cov.sz
00150          + cov.syx * cov.sxz * cov.szy + cov.sx * cov.sy * cov.sz - cov.sx * cov.syz * cov.szy;
00151   if(cov_bla == 0.0)
00152   {
00153     cov.sx = 0.0000001;
00154     cov.sy = 0.0000001;
00155     cov.sz = 0.0000001;
00156     cov_bla = cov.szx * cov.sxy * cov.syz - cov.szx * cov.sxz *  cov.sy - cov.syx * cov.sxy * cov.sz
00157          + cov.syx * cov.sxz * cov.szy + cov.sx * cov.sy * cov.sz - cov.sx * cov.syz * cov.szy;
00158   }
00159   double t1 = (( dx * (cov.sy   * cov.sz  - cov.syz * cov.szy)  / cov_bla)-
00160     ( dy * (-cov.szx * cov.syz + cov.syx * cov.sz)   / cov_bla) -
00161     ( dz * (cov.szx  * cov.sy  - cov.syx * cov.szy)  / cov_bla)) * dx;
00162   double t2 =  ((-dx * (cov.sxy  * cov.sz  - cov.sxz * cov.szy)) / cov_bla +
00163     ( dy * (-cov.szx * cov.sxz + cov.sx  * cov.sz)   / cov_bla) +
00164     ( dz * (cov.szx  * cov.sxy - cov.sx  * cov.szy)  / cov_bla)) * dy;
00165   double t3 = (( dx * (cov.sxy  * cov.syz - cov.sxz * cov.sy)   / cov_bla) -
00166     ( dy * (-cov.syx * cov.sxz + cov.sx  * cov.syz)  / cov_bla) +
00167     ( dz * (-cov.syx * cov.sxy + cov.sx  * cov.sy)   / cov_bla)) * dz;
00168 
00169 
00170   /*printf("cov_bla %f, dx %f dy %f dz %f\n",cov_bla,  dx, dy, dz);*/
00171   det_temp = det(cov);
00172   if(det_temp == 0)
00173     det_temp = 0.00000001;
00174   c_temp = 1.0 /( pow(2.0 * M_PI, 1.5) * sqrt(fabs(det_temp)));
00175   /*printf("t1 %f ,t2 %f, t3 %f, cov_bla = %f\n", t1, t2, t3, cov_bla);*/
00176 
00177   expo = (-1.0/2.0) * (t1 + t2 + t3);
00178 
00179   /*printf("expo = %f\n", expo);*/
00180 
00181   c_temp *= exp(expo);
00182   /*printf("ctemp = %f\n", c_temp);*/
00183   return c_temp;
00184 }
00185 
00186 
00187 Point3D TransformPoint(Point3D hand, Point3D target, double alpha, double beta, double delta)
00188 {
00189   Point3D newp;
00193   newp.x = target.x + (delta - hand.z) * cos(alpha) * sin(beta) - cos(beta) * hand.x - sin(alpha) * sin(beta) * hand.y;
00194   newp.y = target.y - (delta - hand.z) * cos(alpha) * cos(beta) - sin(beta) * hand.x + sin(alpha) * cos(beta) * hand.y;
00195   newp.z = target.z + (delta - hand.z) * sin(alpha) + cos(alpha) * hand.y;
00196   /*printf("Pnew %f %f %f\n",  newp.x, newp.y, newp.z);*/
00197   return newp;
00198 }
00199 
00200 
00201 #define MAX_HAND_POINTS 100
00202 HandConfig GetGraspLM(Point3D* obstacle_list, CovariancePoint* obstacle_covs, int num,
00203                       double offset_rot_z_side, double offset_rot_z_top)
00204 {
00205   HandConfig ret;
00206   Point3D hand_center;
00207   double delta_delta, delta_max, delta_min, delta_beta, beta, delta, min_score, score, alpha, delta_alpha, alpha_min, beta_max, beta_loop;
00208   double min_grasp_prop = 60.0;
00209   int iter_obstacle, iter_hand_point;
00210   Point3D hand_points_trans[MAX_HAND_POINTS];
00211   if(g_data.num_hand_points > MAX_HAND_POINTS)
00212   {
00213     printf("Error: too many hand points passed\n");
00214     g_data.num_hand_points = MAX_HAND_POINTS;
00215   }
00217   hand_center.x = 0.0;
00218   hand_center.y = 0.0;
00219   hand_center.z = 0.0;
00221   ret.alpha = 0.0;
00222   ret.beta = 0.0;
00223   ret.delta_max = 0.0;
00224   g_data.obstacle_list = obstacle_list;
00225   g_data.obstacle_covs = obstacle_covs;
00226   g_data.num_obstacles = num;
00227 
00228 
00229   printf("Obstacle List\n");
00230 
00231   for(iter_obstacle = 0; iter_obstacle < g_data.num_obstacles; iter_obstacle++)
00232   {
00233     Point3D obs = g_data.obstacle_list[iter_obstacle];
00234     CovariancePoint cov = g_data.obstacle_covs[iter_obstacle];
00235     printf("Obs: %05.2f,%05.2f,%05.2f\n", obs.x, obs.y, obs.z);
00236     printf(" Cov: %05.2f,%05.2f,%05.2f\n%05.2f,%05.2f,%05.2f\n%05.2f,%05.2f,%05.2f\n", cov.sx, cov.sxy, cov.sxz, cov.syx, cov.sy, cov.syz, cov.szx, cov.szy, cov.sz);
00237   }
00238 
00239 
00240 
00241 
00243   delta_max = (g_data.max_finger+ (fabs(g_data.obstacle_covs[0].sx) + fabs(g_data.obstacle_covs[0].sy) + fabs(g_data.obstacle_covs[0].sz)) /3.0);
00244 
00246   if(fabs(g_data.obstacle_list[0].z - g_data.table_z) <  g_data.palm_width )
00247   {
00248      printf("Table collision. Limiting alpha to pi/2\n");
00249      alpha_min = M_PI / 2.0;
00250      delta_min = g_data.max_finger - (g_data.obstacle_list[0].z - g_data.table_z);
00251      if(delta_max < delta_min)
00252       delta_max = delta_min + fabs(g_data.obstacle_covs[0].sz);
00253   }
00254   else
00255   {
00256     printf("table is safe : %f (=obj_z - table) >= %f (=palm_width)\n", fabs(g_data.obstacle_list[0].z - g_data.table_z),  g_data.palm_width);
00257     alpha_min = 0.0;
00258     delta_min = min(fabs(g_data.obstacle_covs[0].sx), min( fabs(g_data.obstacle_covs[0].sy) ,fabs(g_data.obstacle_covs[0].sz)))/2.0;
00259   }
00260   /*printf("Delta Max: %f\n", delta_max);*/
00261   delta_delta = (delta_max - delta_min) / 20.0;
00262   delta_beta = 0.0872 * M_PI;
00263   delta_alpha = M_PI/2.0;
00264   beta_max=M_PI/2.0;
00265 
00267   min_score = 100000.0;
00268   for(beta_loop = 0; beta_loop < beta_max; beta_loop += delta_beta)
00269   {
00270     for(alpha = alpha_min; alpha < M_PI / 2.0 +0.1; alpha += delta_alpha)
00271     {
00272       double score_dmax = 0.0;
00273       if(alpha == 0)
00274       {
00275         beta = beta_loop + offset_rot_z_side;
00276       }
00277       else
00278       {
00279         beta = beta_loop +  offset_rot_z_top;
00280       }
00281       for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00282       {
00283         hand_points_trans[iter_hand_point] = TransformPoint(g_data.hand_points[iter_hand_point], g_data.obstacle_list[0], alpha, beta, delta_max);
00284       }
00285       for(iter_obstacle = 0; iter_obstacle < g_data.num_obstacles; iter_obstacle++)
00286       {
00287         Point3D obs = g_data.obstacle_list[iter_obstacle];
00288         CovariancePoint cov = g_data.obstacle_covs[iter_obstacle];
00289         for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00290         {
00291           score_dmax += f( hand_points_trans[iter_hand_point], obs, cov);
00292         }
00293       }
00294       if(alpha <  M_PI / 4.0)
00295         delta_min = min(fabs(g_data.obstacle_covs[0].sx), fabs(g_data.obstacle_covs[0].sy))/2.0;
00296       else
00297       {
00298         delta_min = max(fabs(g_data.obstacle_covs[0].sz)/2.0, g_data.max_finger - (g_data.obstacle_list[0].z - g_data.table_z));
00299         printf("delta min including table: %f\n", delta_min);
00300       }
00301       delta_delta = (delta_max - delta_min) / 20.0;
00302       for(delta = delta_min; delta < delta_max; delta += delta_delta)
00303       {
00304         Point3D hand_center_trans = TransformPoint(hand_center, g_data.obstacle_list[0], alpha, beta, delta);
00305         score = score_dmax;
00306 
00308         for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00309         {
00310           hand_points_trans[iter_hand_point] = TransformPoint(g_data.hand_points[iter_hand_point], g_data.obstacle_list[0], alpha, beta, delta);
00311         }
00312         for(iter_obstacle = 0; iter_obstacle < g_data.num_obstacles; iter_obstacle++)
00313         {
00314           Point3D obs = g_data.obstacle_list[iter_obstacle];
00315           CovariancePoint cov = g_data.obstacle_covs[iter_obstacle];
00316           for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00317           {
00318             score += f( hand_points_trans[iter_hand_point], obs, cov);
00319           }
00320         }
00321 
00322         score -= f( hand_center_trans , g_data.obstacle_list[0], g_data.obstacle_covs[0]) * min_grasp_prop;
00323         /*printf("alpha=%05.3f beta=%05.3f delta=%05.3f score=%f \n",alpha,beta,delta,score);*/
00324 
00325 
00326         if(score < min_score)
00327         {
00328           min_score = score;
00329           ret.delta_max = delta;
00330           ret.beta = beta;
00331           ret.alpha = alpha;
00332         }
00333       }
00334     }
00335   }
00336   return ret;
00337 }
00338 
00339 
00340 HandConfList GetGraspList(Point3D* obstacle_list, CovariancePoint* obstacle_covs, int num,
00341                       double offset_rot_z_side, double offset_rot_z_top)
00342 {
00343   HandConfList ret;
00344   HandConfig max;
00345   ret.length = 0;
00346   ret.min_score_index = 0;
00347 
00348   Point3D hand_center;
00349   double delta_delta, delta_max, delta_min, delta_beta, beta, delta, min_score, score, alpha, delta_alpha, alpha_min, beta_max, beta_loop;
00350   double min_grasp_prop = 60.0;
00351   int iter_obstacle, iter_hand_point;
00352   Point3D hand_points_trans[MAX_HAND_POINTS];
00353   if(g_data.num_hand_points > MAX_HAND_POINTS)
00354   {
00355     printf("Error: too many hand points passed\n");
00356     g_data.num_hand_points = MAX_HAND_POINTS;
00357   }
00359   hand_center.x = 0.0;
00360   hand_center.y = 0.0;
00361   hand_center.z = 0.0;
00363   max.alpha = 0.0;
00364   max.beta = 0.0;
00365   max.delta_max = 0.0;
00366   g_data.obstacle_list = obstacle_list;
00367   g_data.obstacle_covs = obstacle_covs;
00368   g_data.num_obstacles = num;
00369 
00370 
00371   printf("Obstacle List\n");
00372 
00373   for(iter_obstacle = 0; iter_obstacle < g_data.num_obstacles; iter_obstacle++)
00374   {
00375     Point3D obs = g_data.obstacle_list[iter_obstacle];
00376     CovariancePoint cov = g_data.obstacle_covs[iter_obstacle];
00377     printf("Obs: %05.2f,%05.2f,%05.2f\n", obs.x, obs.y, obs.z);
00378     printf(" Cov: %05.2f,%05.2f,%05.2f\n%05.2f,%05.2f,%05.2f\n%05.2f,%05.2f,%05.2f\n", cov.sx, cov.sxy, cov.sxz, cov.syx, cov.sy, cov.syz, cov.szx, cov.szy, cov.sz);
00379   }
00380 
00381 
00382 
00383 
00385   delta_max = (g_data.max_finger+ (fabs(g_data.obstacle_covs[0].sx) + fabs(g_data.obstacle_covs[0].sy) + fabs(g_data.obstacle_covs[0].sz)) /3.0);
00386 
00388   if(fabs(g_data.obstacle_list[0].z - g_data.table_z) < g_data.palm_width )
00389   {
00390      printf("Table collision. Limiting alpha to pi/2\n");
00391      alpha_min = M_PI / 2.0;
00392      delta_min = g_data.max_finger - (g_data.obstacle_list[0].z - g_data.table_z);
00393      if(delta_max < delta_min)
00394       delta_max = delta_min + fabs(g_data.obstacle_covs[0].sz);
00395   }
00396   else
00397   {
00398     printf("table is safe : %f (=obj_z - table) >= %f (=palm_width)\n", fabs(g_data.obstacle_list[0].z - g_data.table_z),  g_data.palm_width);
00399     alpha_min = 0.0;
00400     delta_min = min(fabs(g_data.obstacle_covs[0].sx), min( fabs(g_data.obstacle_covs[0].sy) ,fabs(g_data.obstacle_covs[0].sz)))/2.0;
00401   }
00402   /*printf("Delta Max: %f\n", delta_max);*/
00403   delta_delta = (delta_max - delta_min) / 20.0;
00404   delta_beta = 0.0872 * M_PI;
00405   delta_alpha = M_PI/2.0;
00406   beta_max=M_PI/2.0;
00407 
00409   min_score = 100000.0;
00410   for(beta_loop = 0; beta_loop < beta_max; beta_loop += delta_beta)
00411   {
00412     for(alpha = alpha_min; alpha < M_PI / 2.0 +0.1; alpha += delta_alpha)
00413     {
00414       double score_dmax = 0.0;
00415       if(alpha == 0)
00416       {
00417         beta = beta_loop + offset_rot_z_side;
00418       }
00419       else
00420       {
00421         beta = beta_loop +  offset_rot_z_top;
00422       }
00423       for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00424       {
00425         hand_points_trans[iter_hand_point] = TransformPoint(g_data.hand_points[iter_hand_point], g_data.obstacle_list[0], alpha, beta, delta_max);
00426       }
00427       for(iter_obstacle = 0; iter_obstacle < g_data.num_obstacles; iter_obstacle++)
00428       {
00429         Point3D obs = g_data.obstacle_list[iter_obstacle];
00430         CovariancePoint cov = g_data.obstacle_covs[iter_obstacle];
00431         for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00432         {
00433           score_dmax += f( hand_points_trans[iter_hand_point], obs, cov);
00434         }
00435       }
00436       if(alpha <  M_PI / 4.0)
00437         delta_min = min(fabs(g_data.obstacle_covs[0].sx), fabs(g_data.obstacle_covs[0].sy))/2.0;
00438       else
00439       {
00440         delta_min = max(fabs(g_data.obstacle_covs[0].sz)/2.0, g_data.max_finger - (g_data.obstacle_list[0].z - g_data.table_z));
00441         printf("delta min including table: %f\n", delta_min);
00442       }
00443       delta_delta = (delta_max - delta_min) / 20.0;
00444       for(delta = delta_min; delta < delta_max; delta += delta_delta)
00445       {
00446         Point3D hand_center_trans = TransformPoint(hand_center, g_data.obstacle_list[0], alpha, beta, delta);
00447         score = score_dmax;
00448 
00450         for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00451         {
00452           hand_points_trans[iter_hand_point] = TransformPoint(g_data.hand_points[iter_hand_point], g_data.obstacle_list[0], alpha, beta, delta);
00453         }
00454         for(iter_obstacle = 0; iter_obstacle < g_data.num_obstacles; iter_obstacle++)
00455         {
00456           Point3D obs = g_data.obstacle_list[iter_obstacle];
00457           CovariancePoint cov = g_data.obstacle_covs[iter_obstacle];
00458           for(iter_hand_point = 0; iter_hand_point < g_data.num_hand_points; iter_hand_point++)
00459           {
00460             score += f( hand_points_trans[iter_hand_point], obs, cov);
00461           }
00462         }
00463 
00464         score -= f( hand_center_trans , g_data.obstacle_list[0], g_data.obstacle_covs[0]) * min_grasp_prop;
00465         /*printf("alpha=%05.3f beta=%05.3f delta=%05.3f score=%f \n",alpha,beta,delta,score);*/
00466 
00467         ret.configs[ret.length].delta_max = delta;
00468         ret.configs[ret.length].beta = beta;
00469         ret.configs[ret.length].alpha = alpha;        
00470         ret.scores[ret.length]  = score;     
00471         ret.length++;
00472         if(ret.length > MAX_TESTED_POSES)
00473         {
00474           printf("To many poses tried!!! Increase MAX_TESTED_POSES\n");
00475           return ret;
00476         }
00477         if(score < min_score)
00478         {
00479           ret.min_score_index = ret.length - 1;
00480           min_score = score;
00481           max.delta_max = delta;
00482           max.beta = beta;
00483           max.alpha = alpha;
00484         }
00485       }
00486     }
00487   }
00488   return ret;
00489 }
00490 
00491 
00492 
00493 
00494 
00495 
00496 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


simple_grasp_planner
Author(s): U. Klank
autogenerated on Thu May 23 2013 09:20:22