graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Andrew T. Miller 
00021 //
00022 // $Id: graspitServer.cpp,v 1.7 2009/03/25 22:10:04 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00030 #include <QTextStream>
00031 #include <iostream>
00032 #include "graspitServer.h"
00033 #include "graspitGUI.h"
00034 #include "ivmgr.h"
00035 #include "world.h"
00036 #include "robot.h"
00037 #include "grasp.h"
00038 #include "contact.h"
00039 
00043 ClientSocket::~ClientSocket()
00044 {
00045 #ifdef GRASPITDBG
00046   std::cout << "client socket destroyed"<<std::endl;
00047 #endif
00048 }
00049 
00059 int
00060 ClientSocket::readBodyIndList(std::vector<Body *> &bodyVec)
00061 {
00062   QTextStream os(this);
00063   int i,numBodies,bodNum;
00064   bool ok;
00065   World *world = graspItGUI->getIVmgr()->getWorld();
00066   std::cout << "ReadBodyIndList Line:"<<line.latin1() << std::endl;
00067 
00068   /* if the index list is empty, use every body and send
00069      back the count
00070   */
00071 
00072   if (strPtr == lineStrList.end()) return FAILURE;
00073 
00074   if ((*strPtr).startsWith("ALL")) {
00075     strPtr++;
00076     for (i=0;i<world->getNumBodies();i++)
00077       bodyVec.push_back(world->getBody(i));
00078     std::cout << "Sending num bodies: "<<world->getNumBodies()<<std::endl;
00079     os << world->getNumBodies() << endl;
00080     return SUCCESS;
00081   }
00082   
00083   numBodies = (*strPtr).toInt(&ok);
00084   if (!ok) return FAILURE;
00085   strPtr++;
00086   
00087   for (i=0;i<numBodies;i++) {
00088     if (strPtr == lineStrList.end()) return FAILURE;
00089     bodNum = (*strPtr).toInt(&ok);
00090     if (!ok) return FAILURE;
00091     
00092     if (bodNum>=0 && bodNum<world->getNumBodies()) {
00093       bodyVec.push_back(world->getBody(bodNum));
00094       if (world->getBody(bodNum)==NULL) {
00095         os << "Error: Cannot find body " << bodNum <<"\n";
00096         return FAILURE;
00097       }
00098     }
00099     else {
00100       os << "Error: Cannot find body " << bodNum <<"\n";
00101       return FAILURE;
00102     }
00103     strPtr++;
00104   }
00105   return SUCCESS;
00106 }
00107 
00108 
00118 int
00119 ClientSocket::readRobotIndList(std::vector<Robot *> &robVec)
00120 {
00121   QTextStream os(this);
00122   int i,robNum,numRobots;
00123   bool ok;
00124   World *world = graspItGUI->getIVmgr()->getWorld();
00125   std::cout << "ReadRobotIndList Line:"<<line.latin1() << std::endl;
00126 
00127   /* if the index list is empty, use every robot and send
00128      back the count
00129   */
00130   if (strPtr == lineStrList.end()) return FAILURE;
00131 
00132   if ((*strPtr).startsWith("ALL")) {
00133     strPtr++;
00134     for (i=0;i<world->getNumRobots();i++)
00135       robVec.push_back(world->getRobot(i));
00136     std::cout << "Sending num robots: "<<world->getNumRobots()<<std::endl;
00137     os << world->getNumRobots() << endl;
00138     return SUCCESS;
00139   }
00140   
00141   numRobots = (*strPtr).toInt(&ok);
00142   if (!ok) return FAILURE;
00143   strPtr++;
00144   
00145   for (i=0;i<numRobots;i++) {
00146     if (strPtr == lineStrList.end()) return FAILURE;
00147     robNum = (*strPtr).toInt(&ok);
00148     if (!ok) return FAILURE;
00149     
00150     if (robNum>=0 && robNum<world->getNumRobots()) {
00151       robVec.push_back(world->getRobot(robNum));
00152       if (world->getRobot(robNum)==NULL) {
00153         os << "Error: Cannot find robot " << robNum <<"\n";
00154         return FAILURE;
00155       }
00156     }
00157     else {
00158       os << "Error: Cannot find robot " << robNum <<"\n";
00159       return FAILURE;
00160     }
00161     strPtr++;
00162   }
00163   return SUCCESS;
00164 }
00165 
00175 void
00176 ClientSocket::readClient()
00177 {
00178   int i,numData,numBodies,numRobots;
00179   double time;
00180   std::vector<Body *> bodyVec;
00181   std::vector<Robot *> robVec;
00182 
00183   bool ok;
00184 
00185   while ( canReadLine() ) {
00186     line = readLine();
00187     line.truncate(line.length()-1); //strip newline character
00188     lineStrList =
00189       QStringList::split(' ',line);
00190     strPtr = lineStrList.begin();
00191 
00192 #ifdef GRASPITDBG
00193     std::cout <<"Command parser line: "<<line << std::endl;
00194 #endif
00195     
00196     if (*strPtr == "getContacts") {
00197       strPtr++; if (strPtr == lineStrList.end()) continue;
00198       numData = (*strPtr).toInt(&ok); strPtr++;
00199       if (!ok) continue;
00200 
00201 #ifdef GRASPITDBG
00202       std::cout << "Num data: "<<numData<<std::endl;
00203 #endif
00204 
00205       if (readBodyIndList(bodyVec)) continue;
00206       numBodies = bodyVec.size();
00207       for (i=0;i<numBodies;i++)
00208         sendContacts(bodyVec[i],numData);
00209     }
00210     
00211     else if (*strPtr == "getAverageContacts") {
00212       strPtr++;
00213       if (readBodyIndList(bodyVec)) continue;
00214       numBodies = bodyVec.size();
00215       for (i=0;i<numBodies;i++)
00216         sendAverageContacts(bodyVec[i]);
00217     }
00218     
00219     else if (*strPtr == "getBodyName") {
00220       strPtr++;
00221       if (readBodyIndList(bodyVec)) continue;
00222       numBodies = bodyVec.size();
00223       for (i=0;i<numBodies;i++)
00224         sendBodyName(bodyVec[i]);
00225     }
00226     
00227     else if (*strPtr == "getRobotName") {
00228       strPtr++;
00229       if (readRobotIndList(robVec)) continue;
00230       numRobots = robVec.size();
00231       for (i=0;i<numRobots;i++)
00232         sendRobotName(robVec[i]);
00233     }
00234     
00235     else if (*strPtr == "getDOFVals") {
00236       strPtr++;
00237       if (readRobotIndList(robVec)) continue;
00238       numRobots = robVec.size();
00239       for (i=0;i<numRobots;i++)
00240         sendDOFVals(robVec[i]);
00241     }
00242     
00243     else if (*strPtr == "moveDOFs") {
00244       strPtr++;
00245       readDOFVals();
00246     }
00247     
00248     else if (*strPtr == "render")
00249       graspItGUI->getIVmgr()->getViewer()->render();
00250     
00251     else if (*strPtr == "setDOFForces") {
00252       strPtr++;
00253       if (readRobotIndList(robVec)) continue;
00254       numRobots = robVec.size();
00255       for (i=0;i<numRobots;i++)
00256         if (readDOFForces(robVec[i])==FAILURE) continue;
00257     }
00258     else if (*strPtr == "moveToContacts")
00259       graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->approachToContact(30, true);
00260  
00261     else if ((*strPtr) == "moveDynamicBodies") {
00262       strPtr++;
00263       if (strPtr == lineStrList.end()) ok = FALSE;
00264       else {
00265         time = (*strPtr).toDouble(&ok); strPtr++;
00266       }
00267       if (!ok)
00268         moveDynamicBodies(-1);
00269       else
00270         moveDynamicBodies(time);
00271     }
00272     
00273     else if (*strPtr == "computeNewVelocities") {
00274 
00275 #ifdef GRASPITDBG
00276       std::cout << "cnv" << std::endl;
00277 #endif
00278 
00279       strPtr++; if (strPtr == lineStrList.end()) continue;
00280       time = (*strPtr).toDouble(&ok); strPtr++;
00281       if (!ok) continue;
00282 
00283 #ifdef GRASPITDBG
00284       std::cout << time <<std::endl;
00285 #endif
00286       computeNewVelocities(time);
00287     }
00288     
00289   }
00290 }
00291 
00298 void
00299 ClientSocket::sendAverageContacts(Body* bod)
00300 {
00301   QTextStream os(this);
00302   std::list<Contact *> contactList;
00303   std::list<Contact *>::iterator cp;
00304   int i,numContacts;
00305   double totalWrench[6]={0.0,0.0,0.0,0.0,0.0,0.0};
00306   double *wrench;
00307   vec3 totalLoc = vec3::ZERO;
00308 
00309   numContacts = bod->getNumContacts();
00310   contactList = bod->getContacts();
00311   for (cp=contactList.begin();cp!=contactList.end();cp++) {
00312     wrench = (*cp)->getDynamicContactWrench();
00313     for (i=0;i<6;i++) totalWrench[i] += wrench[i];
00314     totalLoc += (*cp)->getContactFrame().translation();
00315   }
00316 
00317   if (numContacts>1) {
00318     for (i=0;i<6;i++) totalWrench[i] /= numContacts;
00319     totalLoc = totalLoc / numContacts;
00320   }
00321 
00322   os << totalWrench[0]<<" "<<totalWrench[1]<<" "<<totalWrench[2]<<
00323     " "<<totalWrench[3]<<" "<<totalWrench[4]<<" "<<totalWrench[5]<<"\n";
00324   os << totalLoc[0] << " "<<totalLoc[1] << " " <<totalLoc[2]<<"\n";
00325 
00326 }
00327 
00332 void
00333 ClientSocket::sendBodyName(Body* bod)
00334 {
00335   QTextStream os(this);
00336   std::cout << "sending " << bod->getName().latin1() << "\n";
00337   os << bod->getName().latin1() << "\n";
00338 }
00339 
00344 void
00345 ClientSocket::sendRobotName(Robot* rob)
00346 {
00347   QTextStream os(this);
00348   std::cout << "sending " << rob->getName().latin1() << "\n";
00349   os << rob->getName().latin1() << "\n";
00350 }
00351 
00360 void
00361 ClientSocket::sendContacts(Body *bod,int numData)
00362 {
00363   QTextStream os(this);
00364   std::list<Contact *> contactList;
00365   std::list<Contact *>::iterator cp;
00366   vec3 loc;
00367   double err;
00368   double *wrench;
00369 
00370 #ifdef GRASPITDBG
00371   std::cout << "sending numContacts: "<<bod->getNumContacts()<<std::endl;
00372 #endif
00373 
00374   os << bod->getNumContacts()<<"\n";
00375 
00376   contactList = bod->getContacts();
00377   for (cp=contactList.begin();cp!=contactList.end();cp++) {
00378         wrench = (*cp)->getDynamicContactWrench();
00379         loc = (*cp)->getContactFrame().translation();
00380         err = (*cp)->getConstraintError();
00381 
00382     os << wrench[0]<<" "<<wrench[1]<<" "<<wrench[2]<<" "<<wrench[3]<<" "<<
00383       wrench[4]<<" "<<wrench[5]<<"\n";
00384         if (numData > 1) 
00385           os << loc[0] << " "<< loc[1] << " " << loc[2]<<"\n";
00386         if (numData > 2)
00387           os << err << "\n";
00388   }
00389 }
00390 
00396 void
00397 ClientSocket::sendDOFVals(Robot *rob)
00398 {
00399   QTextStream os(this);
00400   int i;
00401 
00402   os << rob->getNumDOF() << "\n";
00403   for (i=0;i<rob->getNumDOF();i++)
00404     os << rob->getDOF(i)->getVal() << "\n";
00405 }
00406 
00415 int
00416 ClientSocket::readDOFVals()
00417 {
00418   Robot *rob;
00419   double *val,*stepby;
00420   QTextStream os(this);
00421   int numDOF,i,robNum;
00422   bool ok=TRUE;
00423 
00424 #ifdef GRASPITDBG
00425   std::cout << "in read dof vals"<<std::endl;
00426 #endif
00427 
00428   if (strPtr == lineStrList.end()) ok=FALSE;
00429   if (ok) robNum = (*strPtr).toInt(&ok);
00430 
00431   if (!ok || robNum < 0 ||
00432     robNum >= graspItGUI->getIVmgr()->getWorld()->getNumRobots()) {
00433         os <<"Error: Robot does not exist.\n";
00434     return FAILURE;
00435   }
00436   rob = graspItGUI->getIVmgr()->getWorld()->getRobot(robNum);
00437 
00438 #ifdef GRASPITDBG
00439   std::cout << "robnum: "<<robNum<<std::endl;
00440 #endif
00441 
00442   strPtr++;
00443   if (strPtr == lineStrList.end()) return FAILURE;
00444 
00445   numDOF=(*strPtr).toInt(&ok);
00446   if (!ok) return FAILURE;
00447   strPtr++;
00448 
00449 #ifdef GRASPITDBG
00450   std::cout << "read robot has: "<< numDOF << " DOF?"<<std::endl;
00451 #endif
00452 
00453   if (numDOF < 1) {
00454 
00455 #ifdef GRASPITDBG
00456         std::cout << "numDOF was zero."<<std::endl;
00457 #endif
00458         return FAILURE;
00459   }
00460   if (numDOF != rob->getNumDOF()) {
00461     os <<"Error: robot has " << rob->getNumDOF() <<" DOF."<<endl;
00462         return FAILURE;
00463   }
00464 
00465   val = new double[numDOF];
00466   stepby = new double[numDOF];
00467 
00468   for (i=0;i<rob->getNumDOF();i++) {
00469         if (strPtr == lineStrList.end()) return FAILURE;
00470         val[i] = (*strPtr).toDouble(&ok);
00471         if (!ok) return FAILURE;
00472         strPtr++;
00473 #ifdef GRASPITDBG
00474         std::cout<<val[i]<<" ";
00475 #endif
00476   }
00477 
00478 #ifdef GRASPITDBG
00479   std::cout<<std::endl;
00480 #endif
00481 
00482   for (i=0;i<rob->getNumDOF();i++) {
00483     if (strPtr == lineStrList.end()) return FAILURE;
00484         stepby[i] = (*strPtr).toDouble(&ok);
00485         if (!ok) return FAILURE;
00486         strPtr++;
00487   }
00488 
00489   rob->moveDOFToContacts(val,stepby,true);
00490 
00491   // these should be separate commands
00492   graspItGUI->getIVmgr()->getWorld()->findAllContacts();
00493   graspItGUI->getIVmgr()->getWorld()->updateGrasps();
00494 
00495   for (i=0;i<rob->getNumDOF();i++) {
00496     os << rob->getDOF(i)->getVal() << "\n";
00497 
00498 #ifdef GRASPITDBG
00499     std::cout << "Sending: "<< rob->getDOF(i)->getVal() << "\n";
00500 #endif
00501   }
00502   delete [] val;
00503   delete [] stepby;
00504   return SUCCESS;
00505 }
00506 
00513 int
00514 ClientSocket::readDOFForces(Robot *rob)
00515 {
00516   double val;
00517   bool ok;
00518  // QTextStream is(this);
00519   QTextStream os(this);
00520   int numDOF,i;
00521 
00522   if (strPtr == lineStrList.end()) return FAILURE;
00523  
00524   numDOF=(*strPtr).toInt(&ok);
00525   if (!ok) return FAILURE;
00526   strPtr++;
00527 
00528 #ifdef GRASPITDBG
00529   std::cout << "read robot has: "<< numDOF << " DOF?"<<std::endl;
00530 #endif
00531 
00532   if (numDOF < 1) {
00533 #ifdef GRASPITDBG
00534     std::cout << "numDOF was zero."<<std::endl;
00535 #endif
00536     return FAILURE;
00537   }
00538   
00539   if (numDOF != rob->getNumDOF()) {
00540     os <<"Error: robot has " << rob->getNumDOF() <<" DOF."<<endl;
00541     return FAILURE;
00542   }
00543   
00544   for (i=0;i<rob->getNumDOF();i++) {
00545     if (strPtr == lineStrList.end()) return FAILURE;
00546     val = (*strPtr).toDouble(&ok);
00547     if (!ok) return FAILURE;
00548     strPtr++;
00549     rob->getDOF(i)->setForce(val);
00550     
00551 #ifdef GRASPITDBG
00552     std::cout<<val<<" ";
00553 #endif
00554   }
00555   
00556 #ifdef GRASPITDBG
00557   std::cout<<std::endl;
00558 #endif
00559   
00560   for (i=0;i<rob->getNumDOF();i++) {
00561     os << rob->getDOF(i)->getForce() << "\n";
00562     
00563 #ifdef GRASPITDBG
00564     std::cout << "Sending: "<< rob->getDOF(i)->getForce() << "\n";
00565 #endif
00566   }
00567   return SUCCESS;
00568 }
00569 
00570 /*
00571 //not finished yet
00572 void
00573 ClientSocket::moveBody(Body *bod)
00574 {
00575   double x,y,z,ax,ay,az,r;
00576 
00577   QTextStream is(this);
00578   is>>x>>y>>z>>ax>>ay>>az>>r;
00579   bod = NULL;  // unused parameter warning
00580 }
00581 */
00582 
00588 void
00589 ClientSocket::moveDynamicBodies(double timeStep)
00590 {
00591   QTextStream os(this);
00592   if (timeStep<0)
00593     timeStep = graspItGUI->getIVmgr()->getWorld()->getTimeStep();
00594 
00595   double actualTimeStep =
00596     graspItGUI->getIVmgr()->getWorld()->moveDynamicBodies(timeStep);
00597   if (actualTimeStep < 0)
00598     os << "Error: Timestep failsafe reached.\n";
00599   else 
00600     os << actualTimeStep << "\n";
00601 }
00602 
00608 void
00609 ClientSocket::computeNewVelocities(double timeStep)
00610 {
00611   QTextStream os(this);
00612   int result = graspItGUI->getIVmgr()->getWorld()->computeNewVelocities(timeStep);
00613   os << result << "\n";
00614 }
00615 
00634 GraspItServer::GraspItServer(Q_UINT16 port, int backlog,
00635                              QObject *parent,const char *name) : 
00636   Q3ServerSocket(port,backlog,parent,name)
00637 {
00638   if (!ok()) {
00639     qWarning("Failed to bind to port");
00640   }
00641 }
00642 
00646 void
00647 GraspItServer::newConnection(int socket)
00648 {
00649   (void)new ClientSocket(socket, this);
00650 
00651 #ifdef GRASPITDBG
00652   std::cout << "new connection" << std::endl;
00653 #endif
00654 }
00655 
00656
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/graspitServer.cpp
