HeadToBodyCenteredRef.cpp

00001 #include <ace/OS_NS_stdio.h>
00002 #include "HeadToBodyCenteredRef.h"
00003 
00004 
00005 
00006 int cartesian_dim=3;
00007 int joint_angle_dim=0;
00008 
00009 HeadToBodyCenteredRef::HeadToBodyCenteredRef(){
00010 
00011 }
00012 
00013 bool HeadToBodyCenteredRef::open(Searchable& s){
00014 Value& struc = s.find("structure");
00015   if(struc.isNull()){
00016     ACE_OS::printf("no robot structure file given\n");
00017     return false;
00018   }
00019   body = new KinematicTree(struc.asString().c_str());
00020   head = new KChainBodySchema(); //KChainOrientationBodySchema if you want to determine visually the rotation
00021   
00022   Value& from = s.find("from");
00023   Value& to = s.find("to");
00024   if(! from.isNull() && ! to.isNull()){
00025       if(!body->LoadChain(from.asString().c_str(),to.asString().c_str(),head)){
00026           cout<<"cannot load chain going from "<<from.asString().c_str()<<" to "<<to.asString().c_str()<<endl;
00027       }
00028   }
00029   else{// default values
00030       body->LoadChain("torso","eyes",head);
00031   }
00032     cout<<head<<endl;
00033     //  head.Load(struc.asString().c_str());
00034   joint_angle_dim = head->GetNbJoints();
00035   cout<<"nb joints "<<joint_angle_dim<<endl;
00036   proprio.Resize(joint_angle_dim);
00037 
00038   //buffer for body schema reading
00039   body_data_size = body->GetTreeSize()*6;
00040   bodydata = new float[body_data_size];
00041 
00042   //opening ports 
00043   if(!in.open(getName("position:i").c_str())){
00044     ACE_OS::printf("Cannot open port %s\n",getName("position:i").c_str());
00045     return false;
00046   }
00047   if(!out.open(getName("position:o").c_str())){
00048     ACE_OS::printf("Cannot open port %s\n",getName("position:o").c_str());
00049     return false;
00050   }
00051   if(!headProprio.open(getName("proprio_head:i").c_str())){
00052     ACE_OS::printf("Cannot open port %s\n",getName("proprio_head:i").c_str());
00053     return false;
00054   }
00055   if(!bodyPort.open(getName("body_schema:i").c_str())){
00056     ACE_OS::printf("Cannot open port %s\n",getName("body_schema:i").c_str());
00057     return false;
00058   }
00059 
00060 if(!cmdPort.open(getName("cmd:i").c_str())){
00061     ACE_OS::printf("Cannot open port %s\n",getName("cmd:i").c_str());
00062     return false;
00063   }
00064   return true;
00065 
00066 
00067 
00068 }
00069 
00070 bool HeadToBodyCenteredRef::close(){
00071   in.close();
00072   out.close();
00073   headProprio.close();
00074   bodyPort.close();
00075   cmdPort.close();
00076   delete[] bodydata;
00077  return true;
00078 }
00079 
00080 
00081 
00082 bool HeadToBodyCenteredRef::updateModule(){
00083   // cart_vec_t in, out;
00084   
00085 
00086   if(cmdPort.ShouldQuit()){return false;}
00087   
00088   // checking if the body schema can be updated
00089   if(bodyPort.ReadBodySchema(bodydata,body_data_size)){
00090     body->GetArticulatedTree()->Deserialize(bodydata,body_data_size);
00091   }
00092 
00093   //getting the latest propioceptive informatin
00094   headProprio.ReadPosition(proprio.GetArray(),joint_angle_dim,true);
00095 
00096   proprio *=deg2rad;
00097   //proprio.Print();
00098 
00099   //checking if an input position has arrived
00100   if(in.ReadPosition(visionPosHead.GetArray())){
00101  
00102     float d = visionPosHead.Norm();
00103     // this is where you can change the signs, but do the same in BodySchemaLearning.cpp
00104     visionPosHead.GetArray()[0] *= -1;
00105     visionPosHead.GetArray()[1] *= -1;
00106     if(d < 1000 && visionPosHead.GetArray()[2] > 100){ //exclude vision outliers 
00107     visionPosHead.Print();
00108  
00109     head->SetForwardKinematics(proprio.GetArray(),visionPosBody.GetArray(), 
00110                                  visionPosHead.GetArray());
00111     visionPosBody.Print();
00112    cout << "------------ "<<joint_angle_dim<<endl;
00113 proprio.Print();
00114    cout << "------------"<<endl;
00115     //  visionPosBody.Print();
00116     //sending out the result
00117     out.SendPosition(visionPosBody.GetArray());
00118     }
00119   }
00120 
00121 
00122   return true;
00123 }
00124 
00125 
00126 int usage(){
00127   cout<<"usage: HeadToBodyModule  [--file <config_file> | --structure <structure_file> --from <head-joint> --to <tail_joint>]"<<endl;
00128   return 1;
00129 }
00130 
00131 int main(int argc, char *argv[]){
00132   HeadToBodyCenteredRef module;
00133   Property prop;
00134   if(argc==1){
00135     return usage();
00136   }
00137    module.runModule(argc,argv);
00138 }
 All Data Structures Functions Variables

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