00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00038
00039 #include <stdio.h>
00040 #include <iostream>
00041
00042
00043
00044 #include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
00045 #include <Inventor/actions/SoGetBoundingBoxAction.h>
00046 #include <Inventor/actions/SoSearchAction.h>
00047 #include <Inventor/nodes/SoCylinder.h>
00048 #include <Inventor/nodes/SoCone.h>
00049 #include <Inventor/nodes/SoCube.h>
00050 #include <Inventor/nodes/SoSphere.h>
00051 #include <Inventor/SoLists.h>
00052 #include "SoArrow.h"
00053 #include "SoTorquePointer.h"
00054 #include "SoComplexShape.h"
00055 #include <Inventor/fields/SoSFFloat.h>
00056 #include <Inventor/actions/SoGetMatrixAction.h>
00057 #include <Inventor/SbLinear.h>
00058 #include <Inventor/nodes/SoRotation.h>
00059 #include <Inventor/nodes/SoCamera.h>
00060 #include <Inventor/nodes/SoTransformSeparator.h>
00061 #include <Inventor/nodes/SoDirectionalLight.h>
00062
00063 #ifdef Q_WS_X11
00064 #include <unistd.h>
00065 #endif
00066
00067
00068 #include "contact.h"
00069 #include "graspitGUI.h"
00070 #include "ivmgr.h"
00071 #include "world.h"
00072 #include "robot.h"
00073 #include "body.h"
00074 #include "matvec3D.h"
00075 #include "grasp.h"
00076
00077
00078 #include <list>
00079
00080
00081 #include "grasp_coordinates.h"
00082 #include "grasp_directions.h"
00083 #include "grasp_preshape.h"
00084 #include "grasp_visualization.h"
00085 #include "grasp_grasps.h"
00086 #include "grasp_planner.h"
00087
00088 #
00089
00090 #ifdef GRASPITDBG
00091 extern FILE *debugfile;
00092 #endif
00093
00094
00095
00096
00097
00101 grasp_planner::grasp_planner()
00102 {
00103
00104 ivmgr = graspItGUI->getIVmgr();
00105
00106 myViewer = ivmgr->getViewer();
00107 my_body = NULL;
00108
00109 nr_of_360_deg_steps = NR_OF_360_DEG_STEPS;
00110 nr_of_parallel_planes_width = NR_OF_PARALLEL_PLANES;
00111 nr_of_parallel_planes_height = NR_OF_PARALLEL_PLANES;
00112 nr_of_parallel_planes_depth = NR_OF_PARALLEL_PLANES;
00113 nr_of_180_deg_grasps = NR_OF_180_DEG_GRASPS;
00114 nr_of_grasp_rotations = NR_OF_GRASP_ROTATIONS;
00115
00116 parameterMode = DEFAULT_PARAMETER_MODE;
00117 }
00118
00122 grasp_planner::~grasp_planner()
00123 {
00124 #ifdef GRASPITDBG
00125 std::cout << "PL_OUT: grasp_planner destroyed." << std::endl;
00126 #endif
00127 }
00128
00129
00138 std::list<plannedGrasp*>
00139 grasp_planner::planIt(GraspableBody* gb,SoGroup *IVPrimitives)
00140 {
00141 #ifdef PLAN_GRASP_ON_ALL_BODIES
00142 int numGB = ivmgr->getWorld()->getNumGB();
00143 #endif
00144 std::list <GraspDirection*> gd_list;
00145 std::list <plannedGrasp*> graspList;
00146 std::list <plannedGrasp*> finalList;
00147 std::list <plannedGrasp*>::iterator gl_it;
00148
00149 my_body = gb;
00150 IVGeomPrimitives = IVPrimitives;
00151
00152 #ifdef PLAN_GRASP_ON_ALL_BODIES
00153
00154 for (int num=0; num<numGB; num++){
00155 my_body = ivmgr->getGB(num);
00156 #ifdef GRASPITDBG
00157 std::cout << "PL_OUT: get Object Nr " << num << std::endl;
00158 #endif
00159 #endif
00160
00161
00162 SoPathList pl = searchPrimitives(my_body);
00163
00164 int length = pl.getLength();
00165 #ifdef GRASPITDBG
00166 std::cout << "PL_OUT: " << length << " primitives found." << std::endl;
00167 #endif
00168
00169 if (length){
00170
00171 #ifdef GRASPITDBG
00172 std::cout << "PL_OUT: Grasp planning started." << std::endl;
00173 #endif
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188 for (int item=0; item<length; item++){
00189 #ifdef GRASPITDBG
00190 std::cout << "PL_OUT: Primitive " << item << std::endl;
00191 #endif
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201 if (parameterMode){
00202
00203 #ifdef GRASPITDBG
00204 std::cout <<
00205 "PL_OUT: Setting the planning parameters for this primitive." <<
00206 std::endl;
00207 #endif
00208 if (!set_planningParametersFromPrimitive(pl[item])) {
00209
00210 #ifdef GRASPITDBG
00211 std::cout << "PL_OUT: Didn't work!" << std::endl;
00212 #endif
00213 }
00214 }
00215
00216
00217
00218
00219
00220
00221 #ifdef GRASPITDBG
00222 std::cout <<
00223 "PL_OUT: getPlannedDirections on this primitive." << std::endl;
00224 #endif
00225 graspList = getPlannedGraspDirections(pl[item]);
00226
00227
00228
00229
00230
00231
00232
00233
00234 #ifdef GRASPITDBG
00235 std::cout <<
00236 "PL_OUT: compute preshapes for this primitive." << std::endl;
00237 #endif
00238 computeGraspPreshapes(graspList, pl[item]);
00239
00240
00241
00242
00243
00244
00245 #ifdef GRASPITDBG
00246 std::cout << "PL_OUT: change coordinate system to global." <<
00247 std::endl;
00248 #endif
00249 localToGlobalCoordinates(graspList, pl[item],gb->getTran());
00250
00251 #ifdef GRASPITDBG
00252 std::cout << "PL_OUT: Copy all grasps to final list." << std::endl;
00253 #endif
00254
00255 for (gl_it=graspList.begin(); gl_it!=graspList.end(); gl_it++){
00256 (*gl_it)->set_graspableBody(my_body);
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266 #if 0
00267 if (!existsInList(**gl_it, finalList))
00268 #endif
00269
00270 finalList.push_back(new plannedGrasp(**gl_it));
00271 }
00272 graspList.clear();
00273 }
00274
00275 #ifdef GRASPITDBG
00276 std::cout<<"PL_OUT: Number of grasps " << finalList.size() << std::endl;
00277 #endif
00278 }
00279
00280 #ifdef GRASPITDBG
00281 else std::cout<<"PL_OUT: No graspable object found in IVtree" << std::endl;
00282 #endif
00283
00284 #ifdef PLAN_GRASP_ON_ALL_BODIES
00285 }
00286 #endif
00287
00288 return finalList;
00289 }
00290
00300 bool
00301 grasp_planner::set_planningParametersFromPrimitive(SoPath* sop)
00302 {
00303 if (parameterMode <= 0)
00304 return false;
00305
00306 if (sop->getTail()->isOfType(SoCylinder::getClassTypeId())){
00307
00308
00309
00310 double radius = ((SoCylinder*)sop->getTail())->radius.getValue();
00311 double height = ((SoCylinder*)sop->getTail())->height.getValue();
00312 switch (parameterMode){
00313 case 1:
00314 nr_of_parallel_planes_height = 1;
00315 nr_of_360_deg_steps = (int)(radius/100.0) * 4 + 4;
00316 nr_of_180_deg_grasps = 1;
00317 nr_of_grasp_rotations = 2;
00318 break;
00319 case 2:
00320 nr_of_parallel_planes_height = 2 * (int)(height/100.0) + 1;
00321 nr_of_360_deg_steps = (int)(radius/100.0) * 8 + 8;
00322 nr_of_180_deg_grasps = 1;
00323 nr_of_grasp_rotations = 4;
00324 break;
00325 case 3:
00326 nr_of_parallel_planes_height = 2 * (int)(height/80.0) + 3;
00327 nr_of_360_deg_steps = (int)(radius/50.0) * 16 + 16;
00328 nr_of_180_deg_grasps = 2;
00329 nr_of_grasp_rotations = 4;
00330 break;
00331 default:
00332 nr_of_parallel_planes_height = 2 * (int)(height/70.0) + 2 * parameterMode + 1;
00333 nr_of_360_deg_steps = (int)(radius/50.0) * 24 + 4 * parameterMode;
00334 nr_of_180_deg_grasps = 2;
00335 nr_of_grasp_rotations = parameterMode + 4;
00336 break;
00337 }
00338 #ifdef GRASPITDBG
00339 std::cout << "PL_OUT: no of parallel planes in height " << nr_of_parallel_planes_height << std::endl;
00340 std::cout << "PL_OUT: no of 360 deg samples " << nr_of_360_deg_steps << std::endl;
00341 std::cout << "PL_OUT: no of 180 deg grasps " << nr_of_180_deg_grasps << std::endl;
00342 std::cout << "PL_OUT: no of grasp rotations " << nr_of_grasp_rotations << std::endl;
00343 #endif
00344 }
00345 else if (sop->getTail()->isOfType(SoCube::getClassTypeId())){
00346
00347
00348
00349 double width = ((SoCube*)sop->getTail())->width.getValue();
00350 double height = ((SoCube*)sop->getTail())->height.getValue();
00351 double depth = ((SoCube*)sop->getTail())->depth.getValue();
00352 switch (parameterMode){
00353 case 1:
00354 nr_of_parallel_planes_width = 1;
00355 nr_of_parallel_planes_height = 1;
00356 nr_of_parallel_planes_depth = 1;
00357 nr_of_180_deg_grasps = 1;
00358 break;
00359 case 2:
00360 nr_of_parallel_planes_width = 2 * (int)(width/100.0) + 1;
00361 nr_of_parallel_planes_height = 2 * (int)(height/100.0) + 1;
00362 nr_of_parallel_planes_depth = 2 * (int)(depth/100.0) + 1;
00363 nr_of_180_deg_grasps = 1;
00364 break;
00365 case 3:
00366 nr_of_parallel_planes_width = 2 * (int)(width/50.0) + 1;
00367 nr_of_parallel_planes_height = 2 * (int)(height/50.0) + 1;
00368 nr_of_parallel_planes_depth = 2 * (int)(depth/50.0) + 1;
00369 nr_of_180_deg_grasps = 2;
00370 break;
00371 default:
00372 nr_of_parallel_planes_width = 2 * (int)(width/50.0) + 2 * parameterMode + 1;
00373 nr_of_parallel_planes_height = 2 * (int)(height/50.0) + 2 * parameterMode + 1;
00374 nr_of_parallel_planes_depth = 2 * (int)(depth/50.0) + 2 * parameterMode + 1;
00375 nr_of_180_deg_grasps = 2;
00376 break;
00377 }
00378 #ifdef GRASPITDBG
00379 std::cout << "PL_OUT: no of parallel planes in height " << nr_of_parallel_planes_height << std::endl;
00380 std::cout << "PL_OUT: no of parallel planes in width " << nr_of_parallel_planes_width << std::endl;
00381 std::cout << "PL_OUT: no of parallel planes in depth " << nr_of_parallel_planes_depth << std::endl;
00382 std::cout << "PL_OUT: no of 180 deg grasps " << nr_of_180_deg_grasps << std::endl;
00383 #endif
00384 }
00385 else if (sop->getTail()->isOfType(SoCone::getClassTypeId())){
00386
00387
00388
00389 double bottomRadius = ((SoCone*)sop->getTail())->bottomRadius.getValue();
00390 double height = ((SoCone*)sop->getTail())->height.getValue();
00391 double side_height = sqrt(bottomRadius * bottomRadius + height * height);
00392 switch (parameterMode){
00393 case 1:
00394 nr_of_parallel_planes_height = 1;
00395 nr_of_360_deg_steps = (int)(bottomRadius/100.0) * 4 + 4;
00396 nr_of_180_deg_grasps = 1;
00397 nr_of_grasp_rotations = 2;
00398 break;
00399 case 2:
00400 nr_of_parallel_planes_height = 2 * (int)(side_height/100.0) + 1;
00401 nr_of_360_deg_steps = (int)(bottomRadius/100.0) * 8 + 8;
00402 nr_of_180_deg_grasps = 1;
00403 nr_of_grasp_rotations = 4;
00404 break;
00405 case 3:
00406 nr_of_parallel_planes_height = 2 * (int)(side_height/80.0) + 3;
00407 nr_of_360_deg_steps = (int)(bottomRadius/50.0) * 16 + 16;
00408 nr_of_180_deg_grasps = 2;
00409 nr_of_grasp_rotations = 4;
00410 break;
00411 default:
00412 nr_of_parallel_planes_height = 2 * (int)(side_height/70.0) + 2 * parameterMode + 1;
00413 nr_of_360_deg_steps = (int)(bottomRadius/50.0) * 24 + 4 * parameterMode;
00414 nr_of_180_deg_grasps = 2;
00415 nr_of_grasp_rotations = parameterMode + 4;
00416 break;
00417 }
00418 #ifdef GRASPITDBG
00419 std::cout << "PL_OUT: no of parallel planes in height " << nr_of_parallel_planes_height << std::endl;
00420 std::cout << "PL_OUT: no of 360 deg samples " << nr_of_360_deg_steps << std::endl;
00421 std::cout << "PL_OUT: no of 180 deg grasps " << nr_of_180_deg_grasps << std::endl;
00422 std::cout << "PL_OUT: no of grasp rotations " << nr_of_grasp_rotations << std::endl;
00423 #endif
00424 }
00425 else if (sop->getTail()->isOfType(SoSphere::getClassTypeId())){
00426
00427
00428
00429 double radius = ((SoSphere*)sop->getTail())->radius.getValue();
00430 switch (parameterMode){
00431 case 1:
00432 nr_of_360_deg_steps = (int)(radius/100.0) * 4 + 4;
00433 nr_of_grasp_rotations = 2;
00434 break;
00435 case 2:
00436 nr_of_360_deg_steps = (int)(radius/100.0) * 8 + 8;
00437 nr_of_grasp_rotations = 4;
00438 break;
00439 case 3:
00440 nr_of_360_deg_steps = (int)(radius/50.0) * 16 + 16;
00441 nr_of_grasp_rotations = 4;
00442 break;
00443 default:
00444 nr_of_360_deg_steps = (int)(radius/50.0) * 24 + 4 * parameterMode;
00445 nr_of_grasp_rotations = parameterMode + 4;
00446 break;
00447 }
00448 #ifdef GRASPITDBG
00449 std::cout << "PL_OUT: no of 360 deg samples " << nr_of_360_deg_steps << std::endl;
00450 std::cout << "PL_OUT: no of grasp rotations " << nr_of_grasp_rotations << std::endl;
00451 #endif
00452 }
00453
00454 return true;
00455 }
00456
00462 int
00463 grasp_planner::determineNumberOfGrasps(SoPathList pl){
00464 int nrOfPrim = pl.getLength();
00465 int nrOfGrasps = 0;
00466
00467 for (int i=0; i<nrOfPrim; i++){
00468
00469 if (pl[i]->getTail()->isOfType(SoCylinder::getClassTypeId())){
00470
00471
00472
00473 nrOfGrasps += nr_of_360_deg_steps * nr_of_parallel_planes_height * nr_of_180_deg_grasps * 2 +
00474 nr_of_grasp_rotations * 2;
00475 }
00476 else if (pl[i]->getTail()->isOfType(SoCube::getClassTypeId())){
00477
00478
00479
00480 nrOfGrasps += nr_of_parallel_planes_width * nr_of_parallel_planes_height * nr_of_180_deg_grasps * 12;
00481 }
00482 else if (pl[i]->getTail()->isOfType(SoCone::getClassTypeId())){
00483
00484
00485
00486 nrOfGrasps += nr_of_360_deg_steps * nr_of_parallel_planes_height * nr_of_180_deg_grasps * 2 +
00487 nr_of_360_deg_steps * nr_of_180_deg_grasps * 2 +
00488 nr_of_grasp_rotations * 2;
00489 }
00490 else if (pl[i]->getTail()->isOfType(SoSphere::getClassTypeId())){
00491
00492
00493
00494
00495 nrOfGrasps += nr_of_grasp_rotations * nr_of_360_deg_steps * nr_of_360_deg_steps;
00496 }
00497 }
00498 return nrOfGrasps;
00499 }
00500
00508 bool
00509 grasp_planner::existsInList(plannedGrasp pg, std::list<plannedGrasp*> pgList)
00510 {
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531 assert(0);
00532 return false;
00533 }
00534
00538 bool
00539 grasp_planner::set_planningParameters(int nr_of_360_deg_steps_in,
00540 int nr_of_parallel_planes_in,
00541 int nr_of_180_deg_grasps_in,
00542 int nr_of_grasp_rotations_in){
00543
00544 if ((nr_of_360_deg_steps_in > 0) &&
00545 ((nr_of_360_deg_steps_in % 2) == 0) &&
00546 ((nr_of_parallel_planes_in > 0) &&
00547 ((nr_of_parallel_planes_in % 2) == 1)) &&
00548 ((nr_of_180_deg_grasps_in == 1) ||
00549 (nr_of_180_deg_grasps_in == 2)) &&
00550 (nr_of_grasp_rotations_in > 0)){
00551
00552 nr_of_360_deg_steps = nr_of_360_deg_steps_in;
00553 nr_of_parallel_planes_width = nr_of_parallel_planes_in;
00554 nr_of_parallel_planes_height = nr_of_parallel_planes_in;
00555 nr_of_parallel_planes_depth = nr_of_parallel_planes_in;
00556 nr_of_180_deg_grasps = nr_of_180_deg_grasps_in;
00557 nr_of_grasp_rotations = nr_of_grasp_rotations_in;
00558 return true;
00559 }
00560 return false;
00561 }
00562
00566 void
00567 grasp_planner::get_planningParameters(int& nr_of_360_deg_steps_in,
00568 int& nr_of_parallel_planes_in,
00569 int& nr_of_180_deg_grasps_in,
00570 int& nr_of_grasp_rotations_in){
00571 nr_of_360_deg_steps_in = nr_of_360_deg_steps;
00572 nr_of_parallel_planes_in = nr_of_parallel_planes_height;
00573 nr_of_180_deg_grasps_in = nr_of_180_deg_grasps;
00574 nr_of_grasp_rotations_in = nr_of_grasp_rotations;
00575 }
00576
00583 SoPathList
00584 grasp_planner::searchPrimitives(GraspableBody* bg)
00585 {
00586 SoPathList pl_ret;
00587 SoPathList pl;
00588 SoSearchAction *saction = new SoSearchAction;
00589 SoGroup *primGeomRoot;
00590 int j;
00591
00592 bg = NULL;
00593 primGeomRoot = IVGeomPrimitives;
00594
00595
00596 saction->setType(SoCylinder::getClassTypeId());
00597 saction->setInterest(SoSearchAction::ALL);
00598 saction->apply(primGeomRoot);
00599
00600 pl = saction->getPaths();
00601 for (j=0; j<pl.getLength(); j++)
00602 {
00603 if (pl[j]->getTail()->isOfType(SoCylinder::getClassTypeId()))
00604 {
00605 pl_ret.append(pl[j]);
00606 }
00607 }
00608
00609
00610 saction->setType(SoCube::getClassTypeId());
00611 saction->apply(primGeomRoot);
00612
00613 pl = saction->getPaths();
00614 for (j=0; j<pl.getLength(); j++)
00615 {
00616 if (pl[j]->getTail()->isOfType(SoCube::getClassTypeId()))
00617 {
00618 pl_ret.append(pl[j]);
00619 }
00620 else pl.remove(j);
00621 }
00622
00623
00624 saction->setType(SoCone::getClassTypeId());
00625 saction->apply(primGeomRoot);
00626
00627 pl = saction->getPaths();
00628 for (j=0; j<pl.getLength(); j++)
00629 {
00630 if (pl[j]->getTail()->isOfType(SoCone::getClassTypeId()))
00631 {
00632 pl_ret.append(pl[j]);
00633 }
00634 }
00635
00636
00637 saction->setType(SoSphere::getClassTypeId());
00638 saction->apply(primGeomRoot);
00639
00640 pl = saction->getPaths();
00641 for (j=0; j<pl.getLength(); j++)
00642 {
00643 if (pl[j]->getTail()->isOfType(SoSphere::getClassTypeId()))
00644 {
00645 pl_ret.append(pl[j]);
00646 }
00647 }
00648
00649 delete saction;
00650 return pl_ret;
00651 }
00652
00658 SoPath*
00659 grasp_planner::getGlobalPath(SoNode *node)
00660 {
00661 SoPathList pl;
00662
00663 SoSearchAction saction;
00664
00665
00666 saction.setNode(node);
00667 saction.setInterest(SoSearchAction::ALL);
00668 saction.apply(ivmgr->getWorld()->getIVRoot());
00669 pl = saction.getPaths();
00670
00671
00672 if (pl.getLength())
00673 return new SoPath(*pl[0]);
00674 #ifdef GRASPITDBG
00675 std::cout << "PL_OUT: Global path not found" << std::endl;
00676 #endif
00677 return NULL;
00678 }
00679
00686 void
00687 grasp_planner::localToGlobalCoordinates(std::list<plannedGrasp*>& graspList,
00688 SoPath *pathToPrim,
00689 const transf &objTran)
00690 {
00691 std::list <plannedGrasp*>::iterator it;
00692 cartesianGraspDirection cgd;
00693 SoGetMatrixAction *gma;
00694 SoTransform *IVprimTran;
00695 transf primTran;
00696
00697 IVprimTran= new SoTransform;
00698 IVprimTran->ref();
00699
00700
00701
00702 gma = new SoGetMatrixAction(myViewer->getViewportRegion());
00703 gma->apply(pathToPrim);
00704 IVprimTran->setMatrix(gma->getMatrix());
00705 primTran.set(IVprimTran);
00706 delete gma;
00707 IVprimTran->unref();
00708
00709
00710 primTran = primTran * objTran;
00711
00712 for (it=graspList.begin(); it!=graspList.end(); it++){
00713
00714
00715
00716 position graspPos = (*it)->get_graspDirection().get_point()+position::ORIGIN;
00717 cgd.set_point(graspPos*primTran - position::ORIGIN);
00718
00719 cgd.set_dir((*it)->get_graspDirection().get_dir()*primTran);
00720
00721
00722 (*it)->set_graspDirection(cgd);
00723 (*it)->set_fixedFingerDirection((*it)->get_fixedFingerDirection()*primTran);
00724 }
00725 }
00726
00733 std::list <plannedGrasp*>
00734 grasp_planner::getPlannedGraspDirections(SoPath* sop)
00735 {
00736
00737 std::list <GraspDirection*> gd_list;
00738 std::list <GraspDirection*>::iterator gd_it;
00739 std::list <plannedGrasp*> graspList;
00740
00741 if (sop->getTail()->isOfType(SoCylinder::getClassTypeId())){
00742
00743
00744
00745 gd_list = getCylinderGraspDirections(sop);
00746 }
00747 else if (sop->getTail()->isOfType(SoCube::getClassTypeId())){
00748
00749
00750
00751 gd_list = getCubeGraspDirections(sop);
00752 }
00753 else if (sop->getTail()->isOfType(SoCone::getClassTypeId())){
00754
00755
00756
00757 gd_list = getConeGraspDirections(sop);
00758 }
00759 else if (sop->getTail()->isOfType(SoSphere::getClassTypeId())){
00760
00761
00762
00763 gd_list = getSphereGraspDirections(sop);
00764 }
00765 #ifdef GRASPITDBG
00766 else std::cout << "PL_OUT: not a valid primitive. " << std::endl;
00767 #endif
00768
00769
00770 for (gd_it=gd_list.begin(); gd_it!=gd_list.end(); gd_it++){
00771
00772
00773
00774 graspList.push_back(new plannedGrasp(*gd_it));
00775 delete (*gd_it);
00776 }
00777
00778 return graspList;
00779
00780 }
00781
00782
00789 std::list <GraspDirection*>
00790 grasp_planner::getCylinderGraspDirections(SoPath* sop)
00791 {
00792
00793 std::list <GraspDirection*> gd_list;
00794 std::list <GraspDirection*>::iterator gd_it;
00795
00796
00797
00798
00799 GraspDirection* cgd;
00800
00801 cylindrical_coordinates ccpoint;
00802 cylindrical_coordinates ccdir;
00803 double radius = ((SoCylinder*)sop->getTail())->radius.getValue();
00804 double height = ((SoCylinder*)sop->getTail())->height.getValue();
00805 double plane_height;
00806
00807
00808 cgd = new cartesianGraspDirection;
00809 ccpoint[0] = 0.0; ccpoint[1] = 0.0; ccpoint[2] = height/2 + STARTING_DISTANCE;
00810 ccdir[0] = 0.0; ccdir[1] = 0.0; ccdir[2] = -1.0;
00811
00812 cgd->set_point(ccpoint.get_pos_cartesian());
00813 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00814 cgd->set_gdType(GDT_CYL_TOP_BOTTOM);
00815
00816 gd_list.push_back(cgd);
00817
00818
00819 cgd = new cartesianGraspDirection;
00820 ccpoint[0] = 0.0; ccpoint[1] = 0.0; ccpoint[2] = (-1) * (height/2 + STARTING_DISTANCE);
00821 ccdir[0] = 0.0; ccdir[1] = 0.0; ccdir[2] = 1.0;
00822
00823 cgd->set_point(ccpoint.get_pos_cartesian());
00824 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00825 cgd->set_gdType(GDT_CYL_TOP_BOTTOM);
00826
00827 gd_list.push_back(cgd);
00828
00829
00830 for (int curr_plane=1; curr_plane<=nr_of_parallel_planes_height; curr_plane++){
00831 plane_height = -height/2 + curr_plane*(height/(nr_of_parallel_planes_height + 1));
00832 for (double step=0.0; step <= 2*M_PI-DELTA_360_DEG_ERROR; step+=(2*M_PI)/nr_of_360_deg_steps){
00833 cgd = new cartesianGraspDirection;
00834 ccpoint[0] = radius + STARTING_DISTANCE; ccpoint[1] = step; ccpoint[2] = plane_height;
00835 ccdir[0] = -1.0; ccdir[1] = 0.0; ccdir[2] = 0.0;
00836
00837 cgd->set_point(ccpoint.get_pos_cartesian());
00838 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00839 cgd->set_gdType(GDT_CYL_SIDES);
00840
00841 gd_list.push_back(cgd);
00842 }
00843 }
00844
00845
00846
00847
00848
00849
00850
00851
00852 for (gd_it=gd_list.begin(); gd_it!=gd_list.end(); gd_it++){
00853
00854 cartesian_coordinates cctmp((*gd_it)->get_point());
00855 double ytmp = cctmp[1];
00856 cctmp[1] = cctmp[2];
00857 cctmp[2] = ytmp;
00858 (*gd_it)->set_point(cctmp);
00859
00860 cctmp = (*gd_it)->get_dir();
00861 ytmp = cctmp[1];
00862 cctmp[1] = cctmp[2];
00863 cctmp[2] = ytmp;
00864 (*gd_it)->set_dir(cctmp);
00865 }
00866 return gd_list;
00867 }
00868
00875 std::list <GraspDirection*>
00876 grasp_planner::getCubeGraspDirections(SoPath* sop)
00877 {
00878
00879 std::list <GraspDirection*> gd_list;
00880 GraspDirection* cgd;
00881
00882
00883
00884
00885
00886 cartesian_coordinates ccpoint;
00887 cartesian_coordinates ccdir;
00888 double width = ((SoCube*)sop->getTail())->width.getValue();
00889 double height = ((SoCube*)sop->getTail())->height.getValue();
00890 double depth = ((SoCube*)sop->getTail())->depth.getValue();
00891 double plane_heightA;
00892 double plane_heightB;
00893 int curr_planeA,curr_planeB;
00894
00895
00896 for (curr_planeA = 1; curr_planeA <= nr_of_parallel_planes_width; curr_planeA++){
00897 for (curr_planeB = 1; curr_planeB <= nr_of_parallel_planes_depth; curr_planeB++){
00898 plane_heightA = - width/2 + curr_planeA * (width/(nr_of_parallel_planes_width + 1));
00899 plane_heightB = - depth/2 + curr_planeB * (depth/(nr_of_parallel_planes_depth + 1));
00900
00901 cgd = new cartesianGraspDirection;
00902 ccpoint[0] = plane_heightA; ccpoint[1] = height + STARTING_DISTANCE; ccpoint[2] = plane_heightB;
00903 ccdir[0] = 0.0; ccdir[1] = -1.0; ccdir[2] = 0.0;
00904
00905 cgd->set_point(ccpoint.get_pos_cartesian());
00906 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00907 cgd->set_gdType(GDT_CUBE_HEIGHT);
00908
00909 gd_list.push_back(cgd);
00910 }
00911 }
00912
00913 for (curr_planeA = 1; curr_planeA <= nr_of_parallel_planes_width; curr_planeA++){
00914 for (curr_planeB = 1; curr_planeB <= nr_of_parallel_planes_depth; curr_planeB++){
00915 plane_heightA = - width/2 + curr_planeA * (width/(nr_of_parallel_planes_width + 1));
00916 plane_heightB = - depth/2 + curr_planeB * (depth/(nr_of_parallel_planes_depth + 1));
00917
00918 cgd = new cartesianGraspDirection;
00919 ccpoint[0] = plane_heightA; ccpoint[1] = -1 * (height + STARTING_DISTANCE); ccpoint[2] = plane_heightB;
00920 ccdir[0] = 0.0; ccdir[1] = 1.0; ccdir[2] = 0.0;
00921
00922 cgd->set_point(ccpoint.get_pos_cartesian());
00923 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00924 cgd->set_gdType(GDT_CUBE_HEIGHT);
00925
00926 gd_list.push_back(cgd);
00927 }
00928 }
00929
00930 for (curr_planeA = 1; curr_planeA <= nr_of_parallel_planes_height; curr_planeA++){
00931 for (curr_planeB = 1; curr_planeB <= nr_of_parallel_planes_depth; curr_planeB++){
00932 plane_heightA = - height/2 + curr_planeA * (height/(nr_of_parallel_planes_height + 1));
00933 plane_heightB = - depth/2 + curr_planeB * (depth/(nr_of_parallel_planes_depth + 1));
00934
00935 cgd = new cartesianGraspDirection;
00936 ccpoint[0] = width + STARTING_DISTANCE; ccpoint[1] = plane_heightA; ccpoint[2] = plane_heightB;
00937 ccdir[0] = -1.0; ccdir[1] = 0.0; ccdir[2] = 0.0;
00938
00939 cgd->set_point(ccpoint.get_pos_cartesian());
00940 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00941 cgd->set_gdType(GDT_CUBE_WIDTH);
00942
00943 gd_list.push_back(cgd);
00944 }
00945 }
00946
00947 for (curr_planeA = 1; curr_planeA <= nr_of_parallel_planes_height; curr_planeA++){
00948 for (curr_planeB = 1; curr_planeB <= nr_of_parallel_planes_depth; curr_planeB++){
00949 plane_heightA = - height/2 + curr_planeA * (height/(nr_of_parallel_planes_height + 1));
00950 plane_heightB = - depth/2 + curr_planeB * (depth/(nr_of_parallel_planes_depth + 1));
00951
00952 cgd = new cartesianGraspDirection;
00953 ccpoint[0] = -1 * (width + STARTING_DISTANCE); ccpoint[1] = plane_heightA; ccpoint[2] = plane_heightB;
00954 ccdir[0] = 1.0; ccdir[1] = 0.0; ccdir[2] = 0.0;
00955
00956 cgd->set_point(ccpoint.get_pos_cartesian());
00957 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00958 cgd->set_gdType(GDT_CUBE_WIDTH);
00959
00960 gd_list.push_back(cgd);
00961 }
00962 }
00963
00964 for (curr_planeA = 1; curr_planeA <= nr_of_parallel_planes_width; curr_planeA++){
00965 for (curr_planeB = 1; curr_planeB <= nr_of_parallel_planes_height; curr_planeB++){
00966 plane_heightA = - width/2 + curr_planeA * (width/(nr_of_parallel_planes_width + 1));
00967 plane_heightB = - height/2 + curr_planeB * (height/(nr_of_parallel_planes_height + 1));
00968
00969 cgd = new cartesianGraspDirection;
00970 ccpoint[0] = plane_heightA; ccpoint[1] = plane_heightB; ccpoint[2] = depth + STARTING_DISTANCE;
00971 ccdir[0] = 0.0; ccdir[1] = 0.0; ccdir[2] = -1.0;
00972
00973 cgd->set_point(ccpoint.get_pos_cartesian());
00974 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00975 cgd->set_gdType(GDT_CUBE_DEPTH);
00976
00977 gd_list.push_back(cgd);
00978 }
00979 }
00980
00981 for (curr_planeA = 1; curr_planeA <= nr_of_parallel_planes_width; curr_planeA++){
00982 for (curr_planeB = 1; curr_planeB <= nr_of_parallel_planes_height; curr_planeB++){
00983 plane_heightA = - width/2 + curr_planeA * (width/(nr_of_parallel_planes_width + 1));
00984 plane_heightB = - height/2 + curr_planeB * (height/(nr_of_parallel_planes_height + 1));
00985
00986 cgd = new cartesianGraspDirection;
00987 ccpoint[0] = plane_heightA; ccpoint[1] = plane_heightB; ccpoint[2] = -1 * (depth + STARTING_DISTANCE);
00988 ccdir[0] = 0.0; ccdir[1] = 0.0; ccdir[2] = 1.0;
00989
00990 cgd->set_point(ccpoint.get_pos_cartesian());
00991 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
00992 cgd->set_gdType(GDT_CUBE_DEPTH);
00993
00994 gd_list.push_back(cgd);
00995 }
00996 }
00997
00998 return gd_list;
00999 }
01000
01007 std::list <GraspDirection*>
01008 grasp_planner::getSphereGraspDirections(SoPath* sop)
01009 {
01010
01011 std::list <GraspDirection*> gd_list;
01012
01013
01014
01015
01016 GraspDirection* cgd;
01017
01018 spherical_coordinates ccpoint;
01019 spherical_coordinates ccdir;
01020 double radius = ((SoSphere*)sop->getTail())->radius.getValue();
01021 double teta, phi;
01022 for (teta=0.0; teta<=M_PI; teta+=M_PI/(nr_of_360_deg_steps/2)){
01023 if (teta == 0.0 || teta >= M_PI){
01024 cgd = new cartesianGraspDirection;
01025 ccpoint[0] = radius + STARTING_DISTANCE; ccpoint[1] = teta; ccpoint[2] = 0.0;
01026 ccdir[0] = -1.0; ccdir[1] = 0.0; ccdir[2] = 0.0;
01027
01028 cgd->set_point(ccpoint.get_pos_cartesian());
01029 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
01030 cgd->set_gdType(GDT_SPH);
01031
01032 gd_list.push_back(cgd);
01033 }
01034 else{
01035 for (phi=0.0; phi <= 2*M_PI; phi+=(2*M_PI)/((int)(nr_of_360_deg_steps * sin(teta))+1)){
01036
01037 cgd = new cartesianGraspDirection;
01038 ccpoint[0] = radius + STARTING_DISTANCE; ccpoint[1] = teta; ccpoint[2] = phi;
01039 ccdir[0] = -1.0; ccdir[1] = 0.0; ccdir[2] = 0.0;
01040
01041 cgd->set_point(ccpoint.get_pos_cartesian());
01042 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
01043 cgd->set_gdType(GDT_SPH);
01044
01045 gd_list.push_back(cgd);
01046 }
01047 }
01048 }
01049 return gd_list;
01050 }
01051
01058 std::list <GraspDirection*>
01059 grasp_planner::getConeGraspDirections(SoPath* sop)
01060 {
01061
01062
01063
01064 std::list <GraspDirection*> gd_list;
01065 std::list <GraspDirection*>::iterator gd_it;
01066
01067
01068
01069
01070 GraspDirection* cgd;
01071
01072 cylindrical_coordinates ccpoint;
01073 cylindrical_coordinates ccdir;
01074 double bottomRadius = ((SoCone*)sop->getTail())->bottomRadius.getValue();
01075 double height = ((SoCone*)sop->getTail())->height.getValue();
01076 double side_height = sqrt(bottomRadius * bottomRadius + height * height);
01077 double bottomAngle = atan2(height, bottomRadius);
01078 double plane_height;
01079
01080
01081 cgd = new cartesianGraspDirection;
01082 ccpoint[0] = 0.0; ccpoint[1] = 0.0; ccpoint[2] = height/2 + STARTING_DISTANCE;
01083 ccdir[0] = 0.0; ccdir[1] = 0.0; ccdir[2] = -1.0;
01084
01085 cgd->set_point(ccpoint.get_pos_cartesian());
01086 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
01087 cgd->set_gdType(GDT_CONE_TOP);
01088
01089 gd_list.push_back(cgd);
01090
01091
01092 cgd = new cartesianGraspDirection;
01093 ccpoint[0] = 0.0; ccpoint[1] = 0.0; ccpoint[2] = (-1) * (height/2 + STARTING_DISTANCE);
01094 ccdir[0] = 0.0; ccdir[1] = 0.0; ccdir[2] = 1.0;
01095
01096 cgd->set_point(ccpoint.get_pos_cartesian());
01097 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
01098 cgd->set_gdType(GDT_CONE_BOTTOM);
01099
01100 gd_list.push_back(cgd);
01101
01102
01103 for (int curr_plane = 1; curr_plane <= nr_of_parallel_planes_height; curr_plane++){
01104 plane_height = curr_plane * (side_height/(nr_of_parallel_planes_height + 1));
01105
01106 for (double step=0.0; step <= 2*M_PI-DELTA_360_DEG_ERROR; step+=(2*M_PI)/nr_of_360_deg_steps){
01107 cgd = new cartesianGraspDirection;
01108 ccpoint[0] = bottomRadius - plane_height * cos(bottomAngle) + STARTING_DISTANCE * sin(bottomAngle);
01109 ccpoint[1] = step;
01110 ccpoint[2] = -height/2 + plane_height * sin(bottomAngle) + STARTING_DISTANCE * cos(bottomAngle);
01111
01112 ccdir[0] = (-1) * sin(bottomAngle);
01113 ccdir[1] = 0.0;
01114 ccdir[2] = (-1) * cos(bottomAngle);
01115
01116 cgd->set_point(ccpoint.get_pos_cartesian());
01117 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
01118 cgd->set_gdType(GDT_CONE_SIDE_PLANE);
01119
01120 gd_list.push_back(cgd);
01121 }
01122 }
01123
01124
01125 for (double step=0.0; step <= 2*M_PI-DELTA_360_DEG_ERROR; step+=(2*M_PI)/nr_of_360_deg_steps){
01126 cgd = new cartesianGraspDirection;
01127 ccpoint[0] = bottomRadius + STARTING_DISTANCE * cos(bottomAngle/2);
01128 ccpoint[1] = step;
01129 ccpoint[2] = -height/2 - STARTING_DISTANCE * sin(bottomAngle/2);
01130
01131 ccdir[0] = (-1) * cos(bottomAngle/2);
01132 ccdir[1] = 0.0;
01133 ccdir[2] = sin(bottomAngle/2);
01134
01135 cgd->set_point(ccpoint.get_pos_cartesian());
01136 cgd->set_dir(ccdir.get_vec_cartesian(ccpoint));
01137 cgd->set_gdType(GDT_CONE_EDGE);
01138
01139 gd_list.push_back(cgd);
01140 }
01141
01142
01143
01144
01145
01146
01147
01148
01149
01150 for (gd_it=gd_list.begin(); gd_it!=gd_list.end(); gd_it++){
01151
01152 cartesian_coordinates cctmp((*gd_it)->get_point());
01153 double ytmp = cctmp[1];
01154 cctmp[1] = cctmp[2];
01155 cctmp[2] = ytmp;
01156 (*gd_it)->set_point(cctmp);
01157
01158 cctmp = (*gd_it)->get_dir();
01159 ytmp = cctmp[1];
01160 cctmp[1] = cctmp[2];
01161 cctmp[2] = ytmp;
01162 (*gd_it)->set_dir(cctmp);
01163 }
01164 return gd_list;
01165 }
01166
01173 void
01174 grasp_planner::computeGraspPreshapes(std::list<plannedGrasp*>& graspList, SoPath* sop)
01175 {
01176 std::list<plannedGrasp*> retList;
01177
01178 if (sop->getTail()->isOfType(SoCylinder::getClassTypeId())){
01179
01180
01181
01182 computeCylinderGraspPreshapes(graspList, sop);
01183 }
01184 else if (sop->getTail()->isOfType(SoCube::getClassTypeId())){
01185
01186
01187
01188 computeCubeGraspPreshapes(graspList, sop);
01189 }
01190 else if (sop->getTail()->isOfType(SoCone::getClassTypeId())){
01191
01192
01193
01194 computeConeGraspPreshapes(graspList, sop);
01195 }
01196 else if (sop->getTail()->isOfType(SoSphere::getClassTypeId())){
01197
01198
01199
01200 computeSphereGraspPreshapes(graspList, sop);
01201 }
01202 #ifdef GRASPITDBG
01203 else std::cout << "PL_OUT: not a valid primitive. " << std::endl;
01204 #endif
01205
01206 return;
01207 }
01208
01216 void
01217 grasp_planner::computeCubeGraspPreshapes(std::list<plannedGrasp*>& graspList, SoPath* sop)
01218 {
01219 preshape comp_preshape;
01220
01221 cartesian_coordinates help;
01222 cartesian_coordinates ffd;
01223 plannedGrasp *pg;
01224 std::list<plannedGrasp*> retList;
01225 std::list<plannedGrasp*>::iterator it;
01226 double width = ((SoCube*)sop->getTail())->width.getValue();
01227 double height = ((SoCube*)sop->getTail())->height.getValue();
01228 double depth = ((SoCube*)sop->getTail())->depth.getValue();
01229
01230 for (it=graspList.begin(); it!=graspList.end(); it++){
01231
01232
01233
01234 if ((*it)->get_graspDirection().get_dir().x() != 0.0)
01235 help = cartesian_coordinates(0.0, 1.0, 0.0);
01236 else if ((*it)->get_graspDirection().get_dir().y() != 0.0)
01237 help = cartesian_coordinates(1.0, 0.0, 0.0);
01238 else if ((*it)->get_graspDirection().get_dir().z() != 0.0)
01239 help = cartesian_coordinates(1.0, 0.0, 0.0);
01240
01241
01242 pg = new plannedGrasp((*it)->get_graspDirection());
01243 comp_preshape.set_preshapeType(PRESHAPE_CUBE_GRASP);
01244 pg->set_preshape(comp_preshape);
01245 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * help);
01246 pg->set_fixedFingerDirection(ffd);
01247
01248 if((ffd[0]!=0.0 && width <= GP_MAX_CUBE_SIZE && width >= GP_MIN_CUBE_SIZE) ||
01249 (ffd[1]!=0.0 && height <= GP_MAX_CUBE_SIZE && height >= GP_MIN_CUBE_SIZE) ||
01250 (ffd[2]!=0.0 && depth <= GP_MAX_CUBE_SIZE && depth >= GP_MIN_CUBE_SIZE)){
01251 retList.push_back(pg);
01252 }
01253 else delete pg;
01254
01255 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01256 pg = new plannedGrasp((*it)->get_graspDirection());
01257 comp_preshape.set_preshapeType(PRESHAPE_CUBE_GRASP);
01258 pg->set_preshape(comp_preshape);
01259 pg->set_fixedFingerDirection(ffd);
01260
01261 if((ffd[0]!=0.0 && width <= GP_MAX_CUBE_SIZE && width >= GP_MIN_CUBE_SIZE) ||
01262 (ffd[1]!=0.0 && height <= GP_MAX_CUBE_SIZE && height >= GP_MIN_CUBE_SIZE) ||
01263 (ffd[2]!=0.0 && depth <= GP_MAX_CUBE_SIZE && depth >= GP_MIN_CUBE_SIZE)){
01264 retList.push_back(pg);
01265 }
01266 else delete pg;
01267
01268 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01269 if (nr_of_180_deg_grasps == 2){
01270 pg = new plannedGrasp((*it)->get_graspDirection());
01271 comp_preshape.set_preshapeType(PRESHAPE_CUBE_GRASP);
01272 pg->set_preshape(comp_preshape);
01273 pg->set_fixedFingerDirection(ffd);
01274
01275 if((ffd[0]!=0.0 && width <= GP_MAX_CUBE_SIZE && width >= GP_MIN_CUBE_SIZE) ||
01276 (ffd[1]!=0.0 && height <= GP_MAX_CUBE_SIZE && height >= GP_MIN_CUBE_SIZE) ||
01277 (ffd[2]!=0.0 && depth <= GP_MAX_CUBE_SIZE && depth >= GP_MIN_CUBE_SIZE)){
01278 retList.push_back(pg);
01279 }
01280 else delete pg;
01281 }
01282
01283 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01284 if (nr_of_180_deg_grasps == 2){
01285 pg = new plannedGrasp((*it)->get_graspDirection());
01286 comp_preshape.set_preshapeType(PRESHAPE_CUBE_GRASP);
01287 pg->set_preshape(comp_preshape);
01288 pg->set_fixedFingerDirection(ffd);
01289
01290 if((ffd[0]!=0.0 && width <= GP_MAX_CUBE_SIZE && width >= GP_MIN_CUBE_SIZE) ||
01291 (ffd[1]!=0.0 && height <= GP_MAX_CUBE_SIZE && height >= GP_MIN_CUBE_SIZE) ||
01292 (ffd[2]!=0.0 && depth <= GP_MAX_CUBE_SIZE && depth >= GP_MIN_CUBE_SIZE)){
01293 retList.push_back(pg);
01294 }
01295 else delete pg;
01296 }
01297 }
01298
01299 for (it=graspList.begin(); it!=graspList.end(); it++){
01300 delete (*it);
01301 }
01302 graspList.clear();
01303
01304 graspList = retList;
01305
01306 return;
01307 }
01308
01317 void
01318 grasp_planner::computeCylinderGraspPreshapes(std::list<plannedGrasp*>& graspList, SoPath* sop)
01319 {
01320 cartesian_coordinates help1;
01321 cartesian_coordinates help2;
01322 cartesian_coordinates ffd;
01323 preshape comp_preshape;
01324 plannedGrasp *pg;
01325 std::list<plannedGrasp*> retList;
01326 std::list<plannedGrasp*>::iterator it;
01327 double radius = ((SoCylinder*)sop->getTail())->radius.getValue();
01328 double height = ((SoCylinder*)sop->getTail())->height.getValue();
01329 double step;
01330
01331 for (it=graspList.begin(); it!=graspList.end(); it++){
01332 if ((*it)->get_graspDirection().get_gdType() == GDT_CYL_TOP_BOTTOM){
01333 if((radius <= GP_MAX_CYL_TBGRASP_DIAMETER) &&
01334 (radius >= GP_MIN_CYL_TBGRASP_DIAMETER)){
01335
01336 comp_preshape.set_preshapeType(PRESHAPE_CYLINDER_TOP_BOTTOM_GRASP);
01337
01338 help1 = cartesian_coordinates(1.0, 0.0, 0.0);
01339 help2 = (*it)->get_graspDirection().get_dir() * help1;
01340
01341 for (int i=0; i< nr_of_grasp_rotations; i++){
01342 step = (double)i*2.0*M_PI/(double)nr_of_grasp_rotations;
01343 pg = new plannedGrasp((*it)->get_graspDirection());
01344 pg->set_preshape(comp_preshape);
01345 ffd = cartesian_coordinates(cos(step) * help1 + sin(step) * help2);
01346 pg->set_fixedFingerDirection(ffd);
01347 retList.push_back(pg);
01348 }
01349 }
01350 }
01351 else if ((*it)->get_graspDirection().get_gdType() == GDT_CYL_SIDES){
01352
01353 comp_preshape.set_preshapeType(PRESHAPE_CYLINDER_SIDE_GRASP);
01354
01355
01356 pg = new plannedGrasp((*it)->get_graspDirection());
01357 pg->set_preshape(comp_preshape);
01358 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * cartesian_coordinates(0.0, 1.0, 0.0));
01359 pg->set_fixedFingerDirection(ffd);
01360 if((radius <= GP_MAX_CYL_SIDEGRASP_DIAMETER) &&
01361 (radius >= GP_MIN_CYL_SIDEGRASP_DIAMETER) &&
01362 (height >= GP_MIN_CYL_SIDEGRASP_HEIGHT))
01363 retList.push_back(pg);
01364 else delete pg;
01365
01366
01367 pg = new plannedGrasp((*it)->get_graspDirection());
01368 pg->set_preshape(comp_preshape);
01369 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01370 pg->set_fixedFingerDirection(ffd);
01371 if (height <= GP_MAX_CYL_SIDEGRASP_HEIGHT)
01372 retList.push_back(pg);
01373 else delete pg;
01374
01375 if (nr_of_180_deg_grasps == 2){
01376
01377 pg = new plannedGrasp((*it)->get_graspDirection());
01378 pg->set_preshape(comp_preshape);
01379 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01380 pg->set_fixedFingerDirection(ffd);
01381 if((radius <= GP_MAX_CYL_SIDEGRASP_DIAMETER) &&
01382 (radius >= GP_MIN_CYL_SIDEGRASP_DIAMETER) &&
01383 (height >= GP_MIN_CYL_SIDEGRASP_HEIGHT))
01384 retList.push_back(pg);
01385 else delete pg;
01386
01387
01388 pg = new plannedGrasp((*it)->get_graspDirection());
01389 pg->set_preshape(comp_preshape);
01390 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01391 pg->set_fixedFingerDirection(ffd);
01392 if (height <= GP_MAX_CYL_SIDEGRASP_HEIGHT)
01393 retList.push_back(pg);
01394 else delete pg;
01395 }
01396 }
01397 }
01398
01399 for (it=graspList.begin(); it!=graspList.end(); it++){
01400 delete (*it);
01401 }
01402 graspList.clear();
01403
01404 graspList = retList;
01405
01406 return;
01407 }
01408
01416 void
01417 grasp_planner::computeSphereGraspPreshapes(std::list<plannedGrasp*>& graspList, SoPath* sop)
01418 {
01419 cartesian_coordinates help1;
01420 cartesian_coordinates help2;
01421 cartesian_coordinates help3;
01422 cartesian_coordinates help4;
01423 cartesian_coordinates ffd;
01424 preshape comp_preshape;
01425 plannedGrasp *pg;
01426 std::list<plannedGrasp*> retList;
01427 std::list<plannedGrasp*>::iterator it;
01428 double radius = ((SoSphere*)sop->getTail())->radius.getValue();
01429 double step;
01430
01431 if((radius <= GP_MAX_SPH_DIAMETER) &&
01432 (radius >= GP_MIN_SPH_DIAMETER)){
01433
01434 comp_preshape.set_preshapeType(PRESHAPE_SPHERE_GRASP);
01435
01436 for (it=graspList.begin(); it!=graspList.end(); it++){
01437
01438
01439
01440 help3 = cartesian_coordinates(1., 0., 0.);
01441 help4 = cartesian_coordinates(0., 1., 1.);
01442 if ((*it)->get_graspDirection().get_dir() % help3 > (*it)->get_graspDirection().get_dir() % help4)
01443 help3 = help4;
01444
01445 help1 = cartesian_coordinates((*it)->get_graspDirection().get_dir() * help3);
01446
01447 help2 = (*it)->get_graspDirection().get_dir() * help1;
01448
01449
01450 for (int i=0; i< nr_of_grasp_rotations; i++){
01451 step = (double)i*2.0*M_PI/(double)nr_of_grasp_rotations;
01452 pg = new plannedGrasp((*it)->get_graspDirection());
01453 pg->set_preshape(comp_preshape);
01454 ffd = cartesian_coordinates(cos(step) * help1 + sin(step) * help2);
01455 pg->set_fixedFingerDirection(ffd);
01456 retList.push_back(pg);
01457 }
01458 }
01459 }
01460
01461 for (it=graspList.begin(); it!=graspList.end(); it++){
01462 delete (*it);
01463 }
01464 graspList.clear();
01465
01466 graspList = retList;
01467
01468 return;
01469 }
01470
01479 void
01480 grasp_planner::computeConeGraspPreshapes(std::list<plannedGrasp*>& graspList, SoPath* sop)
01481 {
01482 cartesian_coordinates help1;
01483 cartesian_coordinates help2;
01484 cartesian_coordinates ffd;
01485 preshape comp_preshape;
01486 plannedGrasp *pg;
01487 std::list<plannedGrasp*> retList;
01488 std::list<plannedGrasp*>::iterator it;
01489 double bottomRadius = ((SoCone*)sop->getTail())->bottomRadius.getValue();
01490 double height = ((SoCone*)sop->getTail())->height.getValue();
01491 double step;
01492
01493 for (it=graspList.begin(); it!=graspList.end(); it++){
01494
01495 if ((*it)->get_graspDirection().get_gdType() == GDT_CONE_TOP){
01496 if((bottomRadius <= GP_MAX_CONE_TGRASP_DIAMETER) &&
01497 (bottomRadius >= GP_MIN_CONE_TGRASP_DIAMETER) &&
01498 (height >= GP_MIN_CONE_TGRASP_HEIGHT)){
01499
01500 comp_preshape.set_preshapeType(PRESHAPE_CONE_TOP_BOTTOM_GRASP);
01501
01502 help1 = cartesian_coordinates(1.0, 0.0, 0.0);
01503 help2 = (*it)->get_graspDirection().get_dir() * help1;
01504
01505 for (int i=0; i< nr_of_grasp_rotations; i++){
01506 step = (double)i*2.0*M_PI/(double)nr_of_grasp_rotations;
01507 pg = new plannedGrasp((*it)->get_graspDirection());
01508 pg->set_preshape(comp_preshape);
01509 ffd = cartesian_coordinates(cos(step) * help1 + sin(step) * help2);
01510 pg->set_fixedFingerDirection(ffd);
01511 retList.push_back(pg);
01512 }
01513 }
01514 }
01515
01516 else if ((*it)->get_graspDirection().get_gdType() == GDT_CONE_BOTTOM){
01517 if((bottomRadius <= GP_MAX_CONE_BGRASP_DIAMETER) &&
01518 (bottomRadius >= GP_MIN_CONE_BGRASP_DIAMETER) &&
01519 (height >= GP_MIN_CONE_BGRASP_HEIGHT)){
01520
01521 comp_preshape.set_preshapeType(PRESHAPE_CONE_TOP_BOTTOM_GRASP);
01522
01523 help1 = cartesian_coordinates(1.0, 0.0, 0.0);
01524 help2 = (*it)->get_graspDirection().get_dir() * help1;
01525
01526 for (int i=0; i< nr_of_grasp_rotations; i++){
01527 step = (double)i*2.0*M_PI/(double)nr_of_grasp_rotations;
01528 pg = new plannedGrasp((*it)->get_graspDirection());
01529 pg->set_preshape(comp_preshape);
01530 ffd = cartesian_coordinates(cos(step) * help1 + sin(step) * help2);
01531 pg->set_fixedFingerDirection(ffd);
01532 retList.push_back(pg);
01533 }
01534 }
01535 }
01536
01537
01538 else if ((*it)->get_graspDirection().get_gdType() == GDT_CONE_SIDE_PLANE){
01539
01540 comp_preshape.set_preshapeType(PRESHAPE_CONE_SIDE_GRASP);
01541
01542
01543 pg = new plannedGrasp((*it)->get_graspDirection());
01544 pg->set_preshape(comp_preshape);
01545 if ((*it)->get_graspDirection().get_dir().x() != 0.0 ||
01546 (*it)->get_graspDirection().get_dir().y() != 1.0 ||
01547 (*it)->get_graspDirection().get_dir().z() != 0.0)
01548 ffd = cartesian_coordinates(normalise((*it)->get_graspDirection().get_dir() * cartesian_coordinates(0.0, 1.0, 0.0)));
01549 else
01550 ffd = cartesian_coordinates(normalise((*it)->get_graspDirection().get_dir() * cartesian_coordinates(1.0, 0.0, 0.0)));
01551
01552 pg->set_fixedFingerDirection(ffd);
01553 if((bottomRadius <= GP_MAX_CONE_SIDEGRASP_DIAMETER) &&
01554 (bottomRadius >= GP_MIN_CONE_SIDEGRASP_DIAMETER) &&
01555 (height >= GP_MIN_CONE_SIDEGRASP_HEIGHT))
01556 retList.push_back(pg);
01557 else delete pg;
01558
01559
01560 pg = new plannedGrasp((*it)->get_graspDirection());
01561 pg->set_preshape(comp_preshape);
01562 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01563 pg->set_fixedFingerDirection(ffd);
01564 if (height <= GP_MAX_CONE_SIDEGRASP_HEIGHT)
01565 retList.push_back(pg);
01566 else delete pg;
01567
01568 if (nr_of_180_deg_grasps == 2){
01569
01570 pg = new plannedGrasp((*it)->get_graspDirection());
01571 pg->set_preshape(comp_preshape);
01572 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01573 pg->set_fixedFingerDirection(ffd);
01574 if((bottomRadius <= GP_MAX_CONE_SIDEGRASP_DIAMETER) &&
01575 (bottomRadius >= GP_MIN_CONE_SIDEGRASP_DIAMETER) &&
01576 (height >= GP_MIN_CONE_SIDEGRASP_HEIGHT))
01577 retList.push_back(pg);
01578 else delete pg;
01579
01580
01581 pg = new plannedGrasp((*it)->get_graspDirection());
01582 pg->set_preshape(comp_preshape);
01583 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01584 pg->set_fixedFingerDirection(ffd);
01585 if (height <= GP_MAX_CONE_SIDEGRASP_HEIGHT)
01586 retList.push_back(pg);
01587 else delete pg;
01588 }
01589 }
01590
01591
01592 else if ((*it)->get_graspDirection().get_gdType() == GDT_CONE_EDGE){
01593
01594 comp_preshape.set_preshapeType(PRESHAPE_CONE_EDGE_GRASP);
01595
01596
01597 pg = new plannedGrasp((*it)->get_graspDirection());
01598 pg->set_preshape(comp_preshape);
01599 if ((*it)->get_graspDirection().get_dir().x() != 0.0 ||
01600 (*it)->get_graspDirection().get_dir().y() != 1.0 ||
01601 (*it)->get_graspDirection().get_dir().z() != 0.0)
01602 ffd = cartesian_coordinates(normalise((*it)->get_graspDirection().get_dir() * cartesian_coordinates(0.0, 1.0, 0.0)));
01603 else
01604 ffd = cartesian_coordinates(normalise((*it)->get_graspDirection().get_dir() * cartesian_coordinates(1.0, 0.0, 0.0)));
01605
01606 pg->set_fixedFingerDirection(ffd);
01607 if((bottomRadius <= GP_MAX_CONE_EDGEGRASP_DIAMETER) &&
01608 (bottomRadius >= GP_MIN_CONE_EDGEGRASP_DIAMETER) &&
01609 (height >= GP_MIN_CONE_EDGEGRASP_HEIGHT))
01610 retList.push_back(pg);
01611 else delete pg;
01612
01613
01614 pg = new plannedGrasp((*it)->get_graspDirection());
01615 pg->set_preshape(comp_preshape);
01616 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01617 pg->set_fixedFingerDirection(ffd);
01618 if (height <= GP_MAX_CONE_EDGEGRASP_HEIGHT)
01619 retList.push_back(pg);
01620 else delete pg;
01621
01622 if (nr_of_180_deg_grasps == 2){
01623
01624 pg = new plannedGrasp((*it)->get_graspDirection());
01625 pg->set_preshape(comp_preshape);
01626 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01627 pg->set_fixedFingerDirection(ffd);
01628 if((bottomRadius <= GP_MAX_CONE_EDGEGRASP_DIAMETER) &&
01629 (bottomRadius >= GP_MIN_CONE_EDGEGRASP_DIAMETER) &&
01630 (height >= GP_MIN_CONE_EDGEGRASP_HEIGHT))
01631 retList.push_back(pg);
01632 else delete pg;
01633
01634
01635 pg = new plannedGrasp((*it)->get_graspDirection());
01636 pg->set_preshape(comp_preshape);
01637 ffd = cartesian_coordinates((*it)->get_graspDirection().get_dir() * ffd);
01638 pg->set_fixedFingerDirection(ffd);
01639 if (height <= GP_MAX_CONE_EDGEGRASP_HEIGHT)
01640 retList.push_back(pg);
01641 else delete pg;
01642 }
01643 }
01644
01645
01646 }
01647
01648
01649 for (it=graspList.begin(); it!=graspList.end(); it++){
01650 delete (*it);
01651 }
01652 graspList.clear();
01653
01654 graspList = retList;
01655
01656 return;
01657 }
01658
01662 int
01663 grasp_planner::get_parameterMode(){
01664 return parameterMode;
01665 }
01666
01670 bool
01671 grasp_planner::set_parameterMode(int in){
01672 if (in >= 0){
01673 parameterMode = in;
01674 return true;
01675 }
01676 return false;
01677 }
01678