ReachingThread Class Reference

Thread imlementing the reaching controller, to ensure that timing are more or less respected. More...

#include <ReachingModuleThread.h>

Public Member Functions

 ReachingThread (int thread_period=50)
 Constructor.
 ~ReachingThread ()
 Destructor.
bool threadInit ()
 Empty shell called at thread launching.
void threadRelease ()
 cleans up allocated memory
bool init (Searchable &s)
 initializes all the variables according to s (given by the command line)
void run ()
 the looping function.
void setModule (Module *mod)
 Set method for the module variable.

Protected Member Functions

bool listen ()
bool sendOutput ()
void sendAnglesFormatVC (joint_vec_t &angles)
ConstString getName (const char *sub=0)
bool matchBodySchemaToRobot ()
bool initMapping (Searchable &s)
void setZeroGains ()

Protected Attributes

KinematicTreebody
KChainBodySchemaarm
 the body schema describing the geometry of the manipulator
Reachingreach
 the class controlling the reaching behavior
bool active_mode
 true of one wants the robot to be active
bool pointing
 true if pointing is allowd
double time_last
 last time position commands were sent
double time_now
 current time
double original_time
 application starting time
float time_granularity
 the simulated period in [ms] between two integration step, i.e.
int nbDOFs
char partname [80]
 the name of the part the module is controlling
int * jointMapping
 the mapping between joint numbers here and for the robot(one may not want to send all the joints values to the robot e.g.
int mappingSize
 the size of this mapping , the number of joints here
Module * module
 the module the thread belongs to
float * bodydata
 buffer used to serialize the body schema
int body_data_size
 size of bodydata
int new_schema
int listen_cnt
int simulation
 if doing only simulation
CartesianDataPort targetPort
 the port that receives the target position
BodySchemaDataPort bodyPort
 the port that receives updates of the body schema
JointAngleDataPort robotJointPort
BufferedPort< Bottle > vcControl_port
 the port for sending the control parameters (gains)
BufferedPort< Bottle > vcFastCommand_port
 the port for sending the joint angle commands
CommandPort cmdPort

Detailed Description

Thread imlementing the reaching controller, to ensure that timing are more or less respected.

Definition at line 212 of file ReachingModuleThread.h.


Constructor & Destructor Documentation

ReachingThread::ReachingThread ( int  thread_period = 50  ) 

Constructor.

Parameters:
thread_period is given in milliseconds. Default value is 50 ms

Definition at line 36 of file ReachingModuleThread.cpp.

References bodydata, and jointMapping.

00036                                            :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 }


Member Function Documentation

bool ReachingThread::init ( Searchable &  s  ) 

initializes all the variables according to s (given by the command line)

Parameters:
s the module options, include: structure, orientation, granularity, name

connection to the thread

normal connection

reading the Kp gains in the conf file

Definition at line 56 of file ReachingModuleThread.cpp.

References active_mode, arm, body_data_size, bodydata, bodyPort, ArticulatedTree::FindAngleRanges(), KinematicTree::GetTreeSize(), jointMapping, KinematicTree::LoadChain(), Reaching::MatchToBodySchema(), original_time, partname, pointing, Reaching::PureJointControl(), Reaching::RandomTarget(), reach, Reaching::SetBodySchema(), simulation, targetPort, time_granularity, time_last, time_now, vcControl_port, and vcFastCommand_port.

00056                                       {
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 }

void ReachingThread::run (  ) 

the looping function.

Checks for new target position commands, integrate the dynamical system and sometimes sends out joint position command

Definition at line 470 of file ReachingModuleThread.cpp.

References active_mode, Reaching::GetTargetAngle(), Reaching::HybridControl(), module, original_time, Reaching::PureJointControl(), reach, Reaching::ReachingStep(), time_granularity, time_last, and time_now.

00470                         {
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 }


Field Documentation

int* ReachingThread::jointMapping [protected]

the mapping between joint numbers here and for the robot(one may not want to send all the joints values to the robot e.g.

virtual rotations)

Definition at line 234 of file ReachingModuleThread.h.

Referenced by init(), ReachingThread(), threadRelease(), and ~ReachingThread().

the simulated period in [ms] between two integration step, i.e.

dt

Definition at line 230 of file ReachingModuleThread.h.

Referenced by init(), and run().


The documentation for this class was generated from the following files:
 All Data Structures Functions Variables

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