ReachingModuleThread.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * Author: Micha Hersch
00006  * email:   micha.hersch@robotcub.org
00007  * website: www.robotcub.org
00008  * Permission is granted to copy, distribute, and/or modify this program
00009  * under the terms of the GNU General Public License, version 2 or any
00010  * later version published by the Free Software Foundation.
00011  *
00012  * A copy of the license can be found at
00013  * http://www.robotcub.org/icub/license/gpl.txt
00014  *
00015  * This program is distributed in the hope that it will be useful, but
00016  * WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00018  * Public License for more details
00019  */
00020 
00021 #define ICUB_IS_HERE
00022 
00023 //#define NO_OUTPUT_CONNECTION
00024 
00025 #define RANDOM_TARGETS
00026 
00027 #include <ace/OS_NS_stdio.h>
00028 #include "ReachingModuleThread.h"
00029 #include "KChainOrientationBodySchema.h"
00030 
00031 
00032 int cartesian_dim=0;
00033 int joint_angle_dim=0;
00034 
00035 
00036 ReachingThread::ReachingThread(int th_period):RateThread(th_period){
00037   jointMapping = NULL;
00038   bodydata = NULL;
00039 #ifdef WITH_HEAD
00040   head = NULL;
00041 #endif
00042   listen_cnt = 0;
00043   //  cnt = 1; //for debugging
00044 
00045 }
00046 
00047 
00048 ReachingThread::~ReachingThread(){
00049     if(jointMapping) delete[] jointMapping;
00050     if(bodydata) delete[] bodydata;
00051     cout<<"thread deleted"<<endl;
00052 }
00053                          
00054 
00055 
00056 bool ReachingThread::init(Searchable &s){
00057   char tmp1[100],tmp2[100];
00058   Value& struc = s.find("structure");
00059   if(struc.isNull()){
00060     ACE_OS::printf("no robot structure file given\n");
00061     return false;
00062   }
00063 
00064   body = new KinematicTree(struc.asString().c_str());
00065 
00066 
00067   int telesc = s.check("pointing");
00068   Value& opt = s.find("orientation");
00069   if(opt.isNull()){
00070       cartesian_dim = 3;
00071     arm = new KChainBodySchema(telesc);
00072     ((KChainBodySchema *)arm)->SetTolerance(100);
00073     // cout<<"orientation is discarded"<<endl;
00074   }
00075   else{
00076     cartesian_dim =6;
00077     arm = new KChainOrientationBodySchema(telesc);
00078     ((KChainOrientationBodySchema *)arm)->SetTolerance(100);
00079     //     cout<<"orientation is considered"<<endl;
00080   }
00081  
00082   simulation = s.check("simulation");
00083   Value& from = s.find("from");
00084   Value& to = s.find("to");
00085   if(! from.isNull() && ! to.isNull()){
00086       if(!body->LoadChain(from.asString().c_str(),to.asString().c_str(),arm)){
00087           cout<<"cannot load chain going from "<<from.asString().c_str()<<" to "<<to.asString().c_str()<<endl;
00088       }
00089   }
00090   else{// default values
00091       body->LoadChain("r_sfe","r_hand",arm);
00092   }
00093     cout<<arm<<endl;
00094 
00095 //     if(!opt.isNull()){
00096 //         if(opt.asInt() == 1){//not yet implemented
00097 //             ((KChainOrientationBodySchema *)arm)->AddVirtualRotationAxis();
00098 //         }
00099 //     }
00100   nbDOFs=joint_angle_dim = arm->GetNbJoints();
00101   arm->UpdateDimension();
00102 
00103   joint_vec_t low_range, up_range;
00104   body->GetArticulatedTree()->FindAngleRanges(low_range.GetArray(),up_range.GetArray(),(KinematicChain *)arm);
00105   arm->SetAngleBounds(low_range,up_range);
00106 
00107 
00108 
00109   cout<<"found "<<joint_angle_dim<<" joints"<<endl;
00110   // arm->Print(cout);
00111   // ! important to do this after joint_angle_dim and cartesian_dim have been set  
00112   reach = new Reaching(); 
00113   reach->SetBodySchema(arm);
00114   cout<<"reaching ok"<<endl;
00115   //  reach->MatchToBodySchema();
00116   // reach->SetStill();
00117 
00118   if (s.check("jointControl")){
00119       reach->PureJointControl();
00120 
00121   }
00122 
00123   if(s.check("pointing")){
00124       pointing = true;
00125       arm->SetIkTrials(1);
00126   }
00127   else{
00128       pointing = false;
00129   }
00130   active_mode =true;
00131 
00132   Value& part = s.find("part");
00133   if(!part.isNull()){
00134       strcpy(partname,part.asString().c_str());
00135   }
00136   else{
00137        strcpy(partname,"right_arm");
00138   }
00139 
00140 
00141   Value& gran = s.find("granularity");
00142   if(gran.isNull()){
00143     time_granularity = 10;
00144   }
00145   else{
00146     time_granularity = gran.asDouble();
00147   }
00148 
00149   initMapping(s);
00150   
00151   //opening intput ports
00152   sprintf(tmp1,"%s",getName("target:i").c_str());
00153   if(!targetPort.open(tmp1)){
00154       ACE_OS::printf("Cannot open port %s\n",tmp1);
00155   }
00156 
00157    sprintf(tmp1,"%s",getName("body_schema:i").c_str());
00158    if(!bodyPort.open(tmp1)){
00159        ACE_OS::printf("Cannot open port %s\n",tmp1);
00160    }
00161 
00162    sprintf(tmp1,"%s",getName("robotState:i").c_str());
00163    if(!robotJointPort.open(tmp1)){
00164        ACE_OS::printf("Cannot open port %s\n",tmp1);
00165    }
00166    sprintf(tmp1,"%s",getName("cmd:i").c_str());
00167    if(!cmdPort.open(tmp1)){
00168        ACE_OS::printf("Cannot open port %s\n",tmp1);
00169    }
00170 
00171 
00172 
00173    cout<<"body size ... "<<endl;
00174    body_data_size = body->GetTreeSize()*6;
00175    bodydata = new float[body_data_size];
00176 
00177    //opening output ports
00178    cout<<"mapping initialized"<<endl;
00179 
00181    sprintf(tmp1,"%s",getName("vc_command:o").c_str());
00182    if(!vcFastCommand_port.open(tmp1)){
00183     ACE_OS::printf("Cannot open port %s\n",tmp1);
00184     return false;
00185   }
00186   
00187 
00188 
00189 #ifdef ICUB_IS_HERE
00190    if(! simulation){
00191    Property options(s.toString());
00192 
00196   ConstString partName(partname);
00197   Property ddOptions;
00198  
00199   /*  
00200   ddOptions.put("robot","icub");
00201   ddOptions.put("device","remote_controlboard");
00202   sprintf(tmp1,getName("encoders/in"));
00203   sprintf(tmp2,"/icub/%s",partName.c_str());
00204   ddOptions.put("local",tmp1);
00205   ddOptions.put("remote",tmp2);
00206 
00207   ddPart = new PolyDriver(ddOptions);
00208 
00209   if(!ddPart->isValid())
00210     {
00211       ACE_OS::printf("Device not available. Here are the known devices:\n");
00212       ACE_OS::printf("%s", Drivers::factory().toString().c_str());
00213       return false;
00214     }
00215 
00217   if(!ddPart->view(PartEncoders))
00218     {
00219       ACE_OS::printf("Cannot view the encoders interface of %s\n",partName.c_str());
00220       return false;
00221     }
00222   
00223   */
00227 
00229 #ifndef NO_OUTPUT_CONNECTION
00230   sprintf(tmp2,"/icub/vc/%s/command",partName.c_str());//was fastCommand
00231     if(!Network::connect(tmp1,tmp2,"udp")){
00232     ACE_OS::printf("Cannot connect port %s to port %s\n",tmp1,tmp2);
00233     return false;
00234   }
00235 #endif    
00236 
00237   sprintf(tmp1,"%s",getName("vc_parameters/out").c_str());
00238   if(!vcControl_port.open(tmp1)){
00239       ACE_OS::printf("Cannot open vcControl port of %s\n",partName.c_str());
00240       return false;
00241     }
00242       
00243 
00244 
00245 #ifndef NO_OUTPUT_CONNECTION  
00246   sprintf(tmp2,"/icub/vc/%s/input",partName.c_str());
00247   if(!Network::connect(tmp1,tmp2)){
00248       ACE_OS::printf("Cannot connect port % to port %s\n",tmp1,tmp2);
00249       return false;
00250     }
00251 #endif
00252     if(options.check("nbDOFs"))
00253         nbDOFs = options.find("nbDOFs").asInt();
00254     else{
00255         ACE_OS::printf("Please specify the nbDOFs of part%s\n",partName.c_str());
00256         return false;
00257     }
00258 
00259     //specifying the gains (also taken from the drumming) 
00261     if(options.check("controlGains"))
00262         {
00263             Bottle& botG = options.findGroup("controlGains");
00264             if(botG.size()!=nbDOFs+1)
00265                 ACE_OS::printf("wrong number of gains\n");
00266             else{
00267                 for(int i=0;i<nbDOFs;i++){
00268                     double gain = botG.get(i+1).asDouble();
00269                     Bottle& cmd = vcControl_port.prepare();
00270                     cmd.clear();
00271                     cmd.addVocab(Vocab::encode("gain"));
00272                     cmd.addInt(jointMapping[i]);
00273                     cmd.addDouble(gain);
00274                     vcControl_port.write(true);
00275                     Time::delay(0.1);
00276                 }
00277             }
00278         }
00279     else{
00280         ACE_OS::printf("no gains defined, using 0\n");
00281     }
00282     if(options.check("maxVelocity")){
00283       Bottle& mv = options.findGroup("maxVelocity");
00284       if(mv.size()!=nbDOFs+1)
00285           ACE_OS::printf("wrong number of max velocity\n");
00286       else{
00287           for(int i=0;i<nbDOFs;i++){
00288               double vel = mv.get(i+1).asDouble();
00289               Bottle& cmd = vcControl_port.prepare();
00290               cmd.clear();
00291               cmd.addVocab(Vocab::encode("svel"));
00292               cmd.addInt(jointMapping[i]);
00293               cmd.addDouble(vel);
00294               vcControl_port.write(true);
00295               Time::delay(0.1);
00296           }
00297       }
00298     }    
00299     else{
00300         ACE_OS::printf("no max velocity defined, using default\n");
00301     }
00302    }
00303 #else
00304 
00305   Value& dest = s.find("destination");
00306   if(!dest.isNull()){
00307     strcpy(tmp1,vcFastCommand_port.getName().c_str());
00308     strcpy(tmp2,dest.asString().c_str());
00309     if(!Network::connect(tmp1,tmp2)){
00310       ACE_OS::printf("Cannot connect port %s to port %s\n",tmp1,tmp2);
00311       return false;
00312     }
00313   }
00314 #endif
00315 
00316   
00317   // updating the posture with the actual one
00318 
00319 
00320   if(!simulation){
00321       if(!matchBodySchemaToRobot()){
00322           return false;
00323       }
00324   }
00325   else{
00326       reach->RandomTarget();
00327   }
00328 
00329   
00330 
00331   reach->MatchToBodySchema();
00332 
00333   
00334   // measuring time
00335   original_time = Time::now();
00336   time_last = 0;
00337   time_now = time_last;
00338   cout<<"initialization done"<<endl;;
00339   return true;
00340 }
00341 
00342 bool ReachingThread::matchBodySchemaToRobot(){
00343     joint_vec_t angles;
00344     if(Network::connect("/icub/right_arm/state:o",
00345                         robotJointPort.getName().c_str())){
00346         if(robotJointPort.ReadPosition(angles.GetArray(),nbDOFs,true)){
00347             cout<<"current position "<<endl;
00348                angles.Print();   
00349                angles *= deg2rad;
00350                arm->SetPosition(angles);
00351                reach->SetTargetAngle(angles);
00352                return true;
00353            }
00354     }
00355     else{
00356       ACE_OS::printf("cannot read robot state\n");;    
00357     }
00358     return false;
00359 }
00360 
00361 //this function is now obsolete 
00362 bool ReachingThread::initMapping(Searchable& s){
00363   Value& mapping = s.find("mapping");
00364   //  cout<<"looking for nb joints"<<endl;
00365   int n=arm->GetNbJoints();
00366   cout<<"found "<<n<<" joints"<<endl;
00367   if(mapping.isNull()){
00368     mappingSize = n;
00369     jointMapping = new int[mappingSize];  
00370     for(int i=0;i<n;i++){
00371       jointMapping[i]=i;
00372     }
00373     return false;
00374   }
00375   else{
00376     Bottle *b =mapping.asList();
00377     int ms = b->size();
00378     mappingSize = min(ms,n);
00379     if(ms != n){
00380       ACE_OS::printf("warning: mapping size %d is not equal to joint size %d",ms,n);
00381     }
00382     jointMapping = new int[mappingSize];  
00383     for(int i=0;mappingSize;i++){
00384       jointMapping[i] = b->get(i).asInt();
00385     }
00386     return true;
00387   }
00388 }
00389 
00390 
00391 
00392 ConstString ReachingThread::getName(const char *sub){
00393   if(module)return module->getName(sub); 
00394   else{ConstString s("/reaching"); return s;}
00395 }
00396 
00397 
00398 
00399 bool ReachingThread::listen(){
00400     cart_vec_t tar;
00401     joint_vec_t head_angles;
00402     int new_t,n;
00403  
00404 
00405   //listening for body schema
00406   n = body->GetTreeSize();
00407   if(bodyPort.ReadBodySchema(bodydata,body_data_size)){//+3 for last link
00408       cout<<"got schema"<<endl;
00409       body->GetArticulatedTree()->Deserialize(bodydata,body_data_size);
00410       new_schema=1;
00411   }
00412 
00413   //listening to target
00414   if(targetPort.getInputCount()>0){
00415       if(cartesian_dim == 3){
00416           new_t = (int) targetPort.ReadPosition(tar.GetArray());
00417       }
00418       else{
00419           new_t = targetPort.ReadPositionAndOrientation(tar.GetArray(),tar.GetArray()+3);
00420       }
00421 
00422     if(new_t){
00423         reach->SetLocalTarget(tar,!pointing);
00424         joint_vec_t ta;
00425         reach->GetTargetAngle(ta);
00426         ta.Print();
00427     }
00428   }
00429   else{// we generate the targets
00430 #ifdef RANDOM_TARGETS
00431       if(reach->TargetReached() || !(rand()%1000)){
00432           reach->RandomTarget(tar,!pointing);
00433           if(tar.Size()==6){
00434               targetPort.SendPositionAndOrientation(tar.GetArray(), tar.GetArray()+3);
00435           }
00436           else{
00437               targetPort.SendPosition(tar.GetArray());
00438           }
00439       }
00440 #endif
00441   }
00442   return true;
00443 }
00444   
00445 
00446 
00447 void ReachingThread::sendAnglesFormatVC(joint_vec_t& angles){
00448    Bottle& cmd = vcFastCommand_port.prepare();
00449    cmd.clear();
00450    for(int i=0;i<mappingSize;i++){
00451 // #ifdef ICUB_IS_HERE not necessary anymore 
00452 //        cmd.addInt(jointMapping[i]);
00453 // #endif 
00454        cmd.addDouble(angles[i]*180/PIf);
00455    }
00456    vcFastCommand_port.write(true);
00457    //   cout<<cmd.toString().c_str()<<endl;
00458 }
00459 
00460 bool ReachingThread::sendOutput(){
00461  joint_vec_t angles;
00462  // body->GetAngles(angles);
00463  reach->GetAnglePosition(angles);
00464  // reach->GetTargetAngle(angles);
00465  sendAnglesFormatVC(angles);
00466  
00467  return true;
00468 }
00469 
00470 void ReachingThread::run(){
00471     Time::delay(0.0001);
00472     //  cout<<"start"<<endl;
00473   time_now = Time::now()-original_time;
00474   if(cmdPort.ShouldQuit()){
00475       Bottle cmd("quit");
00476       Bottle reply;
00477       module->safeRespond(cmd,reply);
00478   }
00479   //  cout<<"listen..."<<endl;
00480   if(++listen_cnt == 20){
00481       listen();
00482       listen_cnt=0; 
00483 }
00484   // debug micha
00485   joint_vec_t ta;
00486   reach->GetTargetAngle(ta);
00487   cout<<"target ";ta.Print();
00488   // end debig micha
00489 
00490   if(active_mode){
00491     float integration_time = (time_now-time_last)*1000 - 0.5*time_granularity; //
00492     for(float tim=0; tim<integration_time; tim+=time_granularity){
00493         switch(new_schema){ // to avoid jumps due to high cartesian velocity because of new schema
00494         case 0: break;
00495         case 1: 
00496         case 2: reach->PureJointControl();new_schema++;break;
00497         case 3: reach->HybridControl();new_schema=0;break;
00498         } 
00499         
00500         
00501         reach->ReachingStep(time_granularity);
00502         // reach->ReachingStep();
00503     }
00504   }
00505     sendOutput();
00506     time_last = time_now;
00507     //   cout<<"stop"<<endl;
00508     //   if(!(cnt--))module->close();//for debugging
00509 }
00510 
00511 bool ReachingThread::threadInit(){
00512   return true;
00513 }
00514 
00515 
00516 void ReachingThread::setZeroGains(){
00517  for(int i=0;i<nbDOFs;i++)
00518             {
00519             Bottle& cmd = vcControl_port.prepare();
00520             cmd.clear();
00521             cmd.addVocab(Vocab::encode("gain"));
00522             cmd.addInt(jointMapping[i]);
00523             cmd.addDouble(0.0);
00524             vcControl_port.write(true);
00525             Time::delay(0.1);
00526             }
00527 
00528     Bottle& cmd = vcControl_port.prepare();
00529     cmd.clear();
00530     cmd.addVocab(Vocab::encode("susp"));
00531     vcControl_port.write(true);
00532 }
00533 
00534 
00535 void ReachingThread::threadRelease(){
00536     if(!simulation){
00537         setZeroGains();
00538     }
00539     //closing all ports
00540      targetPort.close();
00541      bodyPort.close();
00542      robotJointPort.close();
00543      vcFastCommand_port.close();
00544      vcControl_port.close();
00545      cmdPort.close();
00546 
00547     //deallocating memory
00548     if(jointMapping){delete [] jointMapping;}
00549     delete body;
00550     delete arm;
00551     delete[] bodydata;
00552 
00553 
00554 
00555 }
00556 
00557 ReachingModule::ReachingModule():Module(){}
00558 
00559 
00560 
00561 bool ReachingModule::open(Searchable &s){
00562   m_thread.setModule(this);  
00563   if(m_thread.init(s)){
00564     cout<<"starting thread"<<endl;
00565     return m_thread.start();
00566   }
00567   return false;
00568 } 
00569 //  // Time::TurboBoost() not yet
00570 //   bool ok=true;
00571 //   active_mode = true;
00572 //   exit = false;
00573 //   ok *= outport.SetPortName("/proprio");
00574 //   ok *= outport.Start();
00575 //   ok *= targetport.SetPortName("/target/out");
00576 //   ok *= targetport.Start();
00577 //   //  ok *= cmdport.Start();
00578 //   body = new KChainBodySchema();
00579 //   body->Load(body_fname);
00580 //   body->Print(cout);
00581 //   reach->SetBodySchema(body);
00582 //   reach->MatchToBodySchema();
00583 //   reach->RandomTarget();
00584   
00585 //   if(ok){
00586 //     return start();
00587 //   }
00588 //   else{
00589 //     return false;
00590 //   }
00591 // }
00592 
00593 
00594 
00595 //void ReachingModule::listen(){
00596 //   Bottle *b =cmdport.read(false);
00597 //   if(b){
00598 //     int cmd = b->get(0).asInt();
00599 //     switch(cmd){
00600 //     case ReachCmd_Active:
00601 //       active_mode = true;
00602 //       break;
00603 //     case ReachCmd_Passive:
00604 //       active_mode = false;
00605 //       break;
00606 //     case ReachCmd_SetPosition:
00607 //       break;
00608 
00609 //     }
00610 //   }
00611 //}
00612 
00613 
00614 // void ReachingModule::sendOutput(){
00615 //   joint_vec_t angles;
00616 //   body->GetAngles(angles);
00617 //   // reach->GetTargetAngle(angles);
00618 //   outport.SendFeedback(angles.GetArray(), body->GetNbJoints());
00619 // }
00620 
00621 // void ReachingModule::run(){
00622 //   time_now = Time:now();
00623 //   listen();
00624 //   if(reach->TargetReached() || !(rand()%100000)){
00625 //     cart_vec_t tar;
00626 //     reach->RandomTarget(tar);
00627 //     targetport.SendPosition(tar[0],tar[1],tar[2]);
00628 //   }
00629 //   if(active_mode){
00630 //     reach->ReachingStep(time_now-time_last);
00631 //   }
00632 //     sendOutput();
00633 //     time_last = time_now;
00634 // }
00635 
00636 bool ReachingModule::close(){
00637   m_thread.stop();
00638   return true;
00639 }
00640 
00641 
00642 
00643 int usage(){
00644   cout<<"usage: reaching_module [--file <config_file> | --structure <structure_file> ";
00645   cout<<"[--orientation 1] [--name <port_base_name>]] [--jointControl] [--granularity <granularity>]"<<endl;
00646   cout<<"<config_file>: standard icub module configuration file containing the module options"<<endl;
00647   cout<<"<structure_file>: xml file describing the geometry of the manipulator ";
00648   cout<<"(using home-made convention, not D.H.)"<<endl;
00649   cout<<"--jointControl: if you want a pure angular controller"<<endl;
00650   cout<<"<granularity>: the integration constant of the dynamical system in [ms], default value=10"<<endl;
00651   cout<<"--orientation 1: if you want to specify the target orientation as well as position"<<endl;
00652   return 1;
00653 }
00654 
00655 int main(int argc, char *argv[]){
00656   ReachingModule rmod;
00657   Property prop;
00658   if(argc==1){
00659     return usage();
00660   }
00661   prop.fromCommand(argc,argv);
00662   if(prop.check("help")){
00663     return usage();
00664   }
00665   rmod.runModule(argc,argv);
00666   cout<<"closing"<<endl;
00667 }
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:26 2010 for Body_Schema_Learning by  doxygen 1.6.1