ReachingGen.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #include "ReachingGen.h"
00020 
00021 //#define SILENT
00022 
00023 
00024 
00025 //#define REACHING_MAIN
00026 
00027 
00028 #ifdef REACHING_MAIN
00029 #include "KChainBodySchema.h"
00030 
00031 int main(int argc,char *argv[]){
00032     Reaching reach;
00033     BodySchema *body = new KChainBodySchema();
00034     Matrix jac(cartesian_dim,joint_angle_dim);
00035     joint_vec_t angle,angle1;
00036     cart_vec_t pos;
00037     srand(time(NULL));
00038 
00039     body->Load(argv[1]);
00040     //  body->Print(cout);
00041     reach.SetBodySchema(body);
00042 
00043     for(int j=1;j<2;j++){
00044         body->SetRandomAngle(angle);
00045         //   body->SetAnglesInRange(angle1);
00046         //   body->SetPosition(angle1);
00047         //  angle.Print();
00048         body->Angle2Cart(pos);
00049         if(!reach.SetLocalTarget(pos)){
00050             cout<<"can't reach"<<endl ;
00051         }
00052         //  cout<<pos<<endl<<endl;
00053         body->SetRandomAngle(angle1);
00054         //   body->SetAnglesInRange(angle1);
00055         //   body->SetPosition(angle1);
00056         reach.MatchToBodySchema(); 
00057         // while(!reach.TargetReached()){
00058         for(int i=0;i<2;i++){
00059             reach.ReachingStep();
00060             reach.GetPosition(pos,angle);
00061             if(reach.TargetReached()){break;}
00062             if(i==999){
00063                 cout<<endl<<"failed"<<endl;
00064                 reach.Print();
00065                 //    for(int k=0;k<300;k++){
00066                 //      reach.ReachingStep();
00067                 //      reach.GetPosition(pos,angle);
00068                 //      cout<<pos<<" "<<angle<<endl;
00069                 //       }
00070             }
00071         }
00072     }
00073     //  cout<<pos<<"  "<<angle<<endl;
00074 
00075 
00076     //  cout<<(KinematicChain)(*body)<<endl;
00077 
00078     //  ifstream file(argv[1]);
00079     // file>>r;
00080     // cout<<r<<endl;
00081     cout<<"done"<<endl;
00082     return 1;
00083 }
00084   
00085 #endif
00086 
00087 
00088 
00089 
00090 Reaching::Reaching(){
00091     alpha = 0.002; //0.08
00092     beta =0.0005; // 0.02
00093     zero_vel_angle = 1e-4;
00094     out_str = NULL;
00095     body =NULL;
00096     tol = 100;
00097     pure_joint_ctl=0;
00098 #ifdef OBSTACLE_AVOIDANCE
00099     env= NULL;
00100     obstacle_rad = 40;
00101     nu = 0.1;
00102 #endif
00103 
00104 }
00105 
00106 Reaching::~Reaching(){
00107     if (out_str) out_str->close();
00108 }
00109 
00110 void Reaching::SetBodySchema(BodySchema *b){
00111     body = b;
00112     cout<<" dim "<<base_weight_cart.Size()<<" "<<endl;
00113     body->GetWeights(base_weight_cart, base_weight_angle);
00114     weight_cart = base_weight_cart;
00115 }
00116 
00117 BodySchema* Reaching::GetBodySchema(){
00118     return body;
00119 }
00120 
00121 void Reaching::MatchToBodySchema(){
00122     body->GetAngles(pos_angle);
00123     body->Angle2Cart(pos_cart);
00124 }
00125 
00126 
00127 void Reaching::RandomTarget(cart_vec_t& pos,int strict){
00128     joint_vec_t angles;
00129     body->GetRandomAngle(angles);
00130     body->Angle2Cart(angles,pos);
00131     body->SetRandomAngle(angles);
00132     SetLocalTarget(pos,strict);
00133 }
00134 
00135 void Reaching::SetStill(){
00136     tar_angle = pos_angle;
00137     target = pos_cart;
00138 }
00139 
00140 
00141 void Reaching::SetTargetAngle(joint_vec_t& angles){
00142     body->SetAnglesInRange(angles);
00143     tar_angle = angles;
00144     body->Angle2Cart(tar_angle,target);
00145 }
00153 int Reaching::SetLocalTarget(cart_vec_t& new_target,int strict)
00154 {
00155     int res;
00156     joint_vec_t tangle;
00157     body->SetPosition(pos_angle);// trial 
00158     res = body->InverseKinematics(new_target, tangle);
00159     if(res){
00160         target = new_target;
00161 #ifndef SILENT 
00162         cout << "reachable target ";new_target.Print();
00163 #endif
00164     }
00165     else{
00166 #ifndef SILENT 
00167         cout << "unreachable target ";new_target.Print();
00168 #endif
00169         if(strict){
00170             // ignoring unreachable target 
00171             //  tar_angle = pos_angle;
00172             //  target = pos_cart;
00173             return 0;
00174         } 
00175         else{
00176             body->Angle2Cart(target);
00177         }
00178     }
00179     tar_angle = tangle;
00180     return res;
00181 }
00182 
00183 
00184 
00185 
00195 void Reaching::ReachingStep(float dt){
00196     if(dt<EPSILON) return;//no point of doing anything
00197     //  cout<<"DT "<<dt<<endl;
00198     joint_vec_t tmp1,tmp3; // temporary variables
00199     cart_vec_t tmp13;
00200     float dist=0;
00201   
00202     // dt = dt/10; 
00203     //dist = UpdateLocalTarget();
00204     if(dist>tol || isnan(dist)){
00205         SetLocalTarget(target);
00206         cout<<dist<<" "<<tol<<endl;
00207     }
00208     // vite in angle space and cart space
00209     alpha*=dt;beta*=dt;
00210     ViteAngle(tar_angle,pos_angle,v_angle,des_angle,des_v_angle);
00211     ViteCart(target,pos_cart,v_cart,des_cart,des_v_cart);
00212     alpha /= dt; beta/=dt;
00213 
00214     //  cout<<"vite done"<<endl; 
00215 #ifdef OBSTACLE_AVOIDANCE
00216     // find the closest obstacle
00217     float ro,point;
00218     float gamma;
00219     CMatrix4_t ijac;
00220     v4_clear(des_a_angle_obs);
00221     if(env){
00222         for (i=1;i<3;i++){
00223             for(int j=0;j<env->CountManipulableObjects();j++){
00224                 LinkDistanceToObject(i,env->GetObject(j),&ro,&point,tmp13);
00225                 //      coutvec(tmp13);
00226                 IntermediateJacobian(i,point,pos_angle,ijac);
00227                 m3_4_t_v_multiply(ijac,tmp13,tmp1);
00228                 // eq. 18 of Khatib,icra'85
00229                 if(ro<=obstacle_rad){
00230                     gamma = nu *(1/ro -1/obstacle_rad)/(ro*ro); //(ro*ro) 
00231                     //test
00232                     //   gamma *= -v4_dot(tmp1,v_angle)/50;
00233                     //   gamma = max(0.0,gamma);
00234                 }
00235                 else{
00236                     gamma =0;
00237                 }
00238                 v4_scale(tmp1,gamma,tmp1);
00239                 v4_add(des_a_angle_obs,tmp1,des_a_angle_obs);
00240             }
00241         }
00242         v4_add(des_a_angle_obs,des_v_angle,des_v_angle);
00243         v4_add(pos_angle,des_v_angle,des_angle);
00244     }
00245 #endif
00246  
00247     body->SetAnglesInRange(des_angle);
00248     des_v_angle = des_angle - pos_angle;
00249     des_v_cart = des_cart - pos_cart;
00250     if(pure_joint_ctl){
00251         tmp1 = des_angle;
00252     }
00253     else{
00254         UpdateWeights();
00255         // coherence enforcement
00256         ProjectVector(des_v_cart,des_v_angle,tmp3);
00257         tmp1 = pos_angle+tmp3;
00258     }
00259     body->SetAnglesInRange(tmp1);
00260     body->Angle2Cart(tmp1,tmp13);
00261     v_angle = tmp1 - pos_angle;
00262     v_cart = tmp13 - pos_cart;
00263     pos_cart = tmp13;
00264     pos_angle = tmp1;
00265     pos_angle.Print();
00266     pos_cart.Print();
00267 }
00268 
00269 
00270 void Reaching::ProjectVector(cart_vec_t& vcart,joint_vec_t& vangle, joint_vec_t& out){
00271     Matrix jac, 
00272         jact, m31,m32(cartesian_dim,cartesian_dim,false),
00273         wa(joint_angle_dim,joint_angle_dim,false); 
00274     joint_vec_t v41,v42;
00275     cart_vec_t v31,v32;
00276     body->Jacobian(pos_angle,jac); //J
00277     jact = jac.Transpose();
00278     wa.Diag(weight_angle);
00279     v31= vcart-jac*vangle;
00280     m31 = jac*wa*jact;
00281     m31 += m32.Diag(weight_cart);
00282     m32 = m31.Inverse();
00283     if(!Matrix::IsInverseOk()){cout<<"bad inversion"<<endl;}
00284     out = vangle+wa*jact*m32*v31;
00285 }
00286 
00287 
00288 
00289 
00298 void Reaching::ViteCart(cart_vec_t& target,cart_vec_t& pos,cart_vec_t& speed,
00299                         cart_vec_t& new_pos, cart_vec_t& new_speed) {
00300     //  cart_vec_t tmp1,tmp2;
00301         //vite in cart space
00302         // v update
00303     new_speed  = speed + ((target-pos)*beta - speed)*alpha;
00304     new_pos = pos+new_speed;
00305     //  cout<<"new cart speed "<<speed<<endl;
00306 }
00307 
00308 
00317 void Reaching::ViteAngle(joint_vec_t& target,joint_vec_t& pos,joint_vec_t& speed,
00318                          joint_vec_t& new_pos, joint_vec_t& new_speed) {
00319         //vite in joint space
00320         // v update
00321         new_speed  = speed + ((target-pos)*beta - speed)*alpha;
00322         new_pos = pos+new_speed;
00323 }
00324 
00325 void Reaching::UpdateWeights(){
00326     joint_vec_t& min_angle = body->GetAnglesLowerBound();
00327     joint_vec_t& max_angle = body->GetAnglesUpperBound();
00328     float range;
00329     for(int i=0;i<joint_angle_dim;i++){
00330         range = max_angle[i]-min_angle[i];
00331         if(range>EPSILON){
00332             weight_angle[i] = 0.5*base_weight_angle[i]*//
00333                 (cos((pos_angle[i] - min_angle[i])*2*PIf/range
00334                      +PIf)+1);
00335             weight_angle[i]*=0.1;   
00336         }
00337         else{
00338             weight_angle[i] = 1.f/EPSILON;
00339         }
00340     }
00341 //     if(pure_joint_ctl){
00342 //         weight_angle *= 0.0f;
00343 //     }
00344 }
00345 
00346 
00347 
00348 float Reaching::UpdateLocalTarget(cart_vec_t *new_target){
00349     cart_vec_t diff,v1,v2;
00350     joint_vec_t v3,v4,v5;
00351     Matrix m1,m2;
00352     Matrix jac,jacT,m3,m4;
00353     float f;
00354     if(new_target){
00355         diff = *new_target-target;
00356         if(diff.Norm2()>tol*tol){
00357             return SetLocalTarget(*new_target);
00358             //      return -1.0f;
00359         }
00360         else{
00361             //updating cartesian target
00362             target=*new_target;
00363         }
00364     }
00365     body->Angle2Cart(tar_angle,v1);
00366     v2=target-v1;
00367     body->Jacobian(tar_angle,jac);
00368      jac.Inverse(m3);//todo : find the weighted version
00369 
00370     if(!Matrix::IsInverseOk()){cout<<"bad pseudo-inverse in Reaching::UpdateLocalTarget"<<endl;}
00371     v3 = m3*v2*0.1;
00372     m4 = m3*jac;
00373     m3.Resize(tar_angle.Size(),tar_angle.Size());
00374     m3.Identity();
00375     v5=(m4-m3)*(tar_angle-pos_angle);
00376     if((f=v5.Norm2())>0.01*0.01){
00377         v5*= 0.001/sqrtf(f);
00378     }
00379     v4 = v5+v3; 
00380     v3 = tar_angle+v4;
00381  
00382     if(body->AnglesInRange(v3)){
00383         tar_angle=v3;
00384     }
00385     //checking precision
00386     body->Angle2Cart(tar_angle,v1);
00387     return body->CartesianDistance(target,v1); 
00388 }
00389 
00390 
00391 
00392 
00393 #ifdef NOT_YET
00394 
00395 
00396 
00397 
00398 void Reaching::SetIncrement(float new_incr){
00399   incr=new_incr;
00400 }
00401 
00402 /***********************************************
00403  * method SetShoulderPosition
00404  *
00405  * sets the shoulder position with respoect to an 
00406  * absolute referential (the same one as SetTarget).
00407  * this referential is oriented like Eric's (x:left,
00408  * y: up, z:front)
00409  *************************************************/
00410 
00411 
00412 void Reaching::SetShoulderPosition(cart_vec_t& pos){
00413   v_copy(pos,shoulder_abs_pos);
00414 }
00415 
00416 void Reaching::GetWeights(cart_vec_t& w_cart, joint_vec_t& w_angle)const{
00417 #ifdef DYN_WEIGHTS
00418   v4_copy(base_weight_angle,w_angle);
00419   v_copy(base_weight_cart,w_cart);
00420 #else
00421   v4_copy(weight_angle,w_angle);
00422   v_copy(weight_cart,w_cart);
00423 #endif
00424 }
00425 void Reaching::SetWeights(cart_vec_t& w_cart, joint_vec_t& w_angle){
00426   v_copy(w_cart,weight_cart);
00427   v4_copy(w_angle,weight_angle);
00428 #ifdef DYN_WEIGHTS
00429   v_copy(w_cart,base_weight_cart);
00430   v4_copy(w_angle,base_weight_angle);
00431 #endif    
00432 }
00433 
00434 
00435 void Reaching::DecrementAngleWeights(float factor){
00436 #ifdef DYN_WEIGHTS
00437   v4_scale(base_weight_angle,factor,base_weight_angle);
00438 #else
00439   v4_scale(weight_angle,factor,weight_angle);
00440 #endif
00441 }
00442 
00443 void Reaching::IncrementAngleWeights(float factor){
00444   float f = factor;
00445   if (f<0) return;
00446 #ifdef DYN_WEIGHTS
00447   v4_scale(base_weight_angle,f,base_weight_angle);
00448 #else
00449   v4_scale(weight_angle,f,weight_angle);
00450 #endif
00451 }
00452 
00453 void Reaching::IncrementCartWeights(float factor){
00454   if (factor<0) return;
00455 #ifdef DYN_WEIGHTS
00456   v_scale(base_weight_cart,factor,base_weight_cart);
00457 #else
00458   v_scale(weight_cart,factor,weight_cart);
00459 #endif
00460 }
00461 
00466 void Reaching::GetTarget(cart_vec_t& tar) const {
00467   Local2AbsRef(target,tar);
00468 }
00469 
00477 int Reaching::SetTarget(cart_vec_t& new_target,int strict){
00478   cart_vec_t local_tar;
00479   Abs2LocalRef(new_target,local_tar);
00480   return SetLocalTarget(local_tar,strict);
00481 }
00482 
00483 
00484 
00485 int Reaching::UpdateTarget(){
00486   if(GetVisualMode()){
00487     return SetTarget(target);
00488   }
00489   else{
00490     return 0;
00491   }
00492 }
00493 
00494 
00495 
00496 int Reaching::SetArmConfigurationTarget(joint_vec_t& new_target)
00497 {
00498   if(AcceptablePosition(new_target)){
00499     ArmConfig_t config;
00500     CVector2ArmConfig(new_target,&config);
00501     configManifold.clear();
00502     configManifold.push_back(config);
00503     Angle2Cart(new_target,target);
00504     return 1;
00505   }
00506   else{
00507 #ifndef SILENT
00508     cout << "unreachable position: ";
00509     coutvec4(new_target);
00510 #endif
00511     return 0;
00512   }
00513 }
00514 
00515 
00516 
00517 
00518 /**********************************************
00519 // method ComputeConfigManifold
00520 //
00521 // Computes all the joint angle positions that
00522 // correpond to a target location. If the target
00523 // is too far, computes the positions that point
00524 // to the target. A sampling is performed, whose
00525 // coarsness in defined by <incr>.
00526 
00527 @arg cart_goal goal in local FoF.
00528 // returns:
00529 // 1 if the target is reachable
00530 // 0 otherwise
00531 ************************************************/
00532 
00533 
00534 
00535 
00536 int Reaching::ComputeConfigManifold(cart_vec_t& cart_goal)
00537 {  
00538   float alpha;
00539   ArmConfig_t tmpconfig;
00540 #ifdef BODY_SCHEMA
00541   joint_vec_t& angles;
00542 #endif
00543 
00544   configManifold.clear();
00545 
00546   for (alpha=-pi;alpha<pi;alpha+=incr){
00547 #ifndef BODY_SCHEMA
00548     if(InvKinematics(cart_goal,alpha,tol,&tmpconfig)){
00549     configManifold.push_back(tmpconfig);
00550     }
00551 #else
00552     if(body->InvKinematics(cart_goal,alpha,tol,angles)){
00553      tmpconfig.FromVector(angles);
00554      configManifold.push_back(tmpconfig);
00555     }
00556  
00557 #endif
00558   }
00559   return (configManifold.size()>0);
00560 }  
00561 
00562 
00563 //int Reaching::DumpConfigManifold(char *filename)
00564 
00565 /******************************
00566 // method Hoap2Rob
00567 //
00568 // Converts joint angles from Hoap deg referential to 
00569 // Robota rad referential
00570 ******************************/
00571 
00572 
00573 void Reaching::Hoap2Rob(pArmConfig_t conf)
00574 {
00575   conf->sfe = -conf->sfe*deg2rad+pi/2;
00576   conf->saa = conf->saa*deg2rad+pi/2;
00577   conf->shr = conf->shr*deg2rad;
00578   conf->eb = -conf->eb*deg2rad;
00579 }
00580 void Reaching::Hoap2Rob(joint_vec_t& conf)
00581 {
00582   conf[0] = -conf[0]*deg2rad+pi/2;
00583   conf[1] = conf[1]*deg2rad+pi/2;
00584   conf[2] = conf[2]*deg2rad;
00585   conf[3] = -conf[3]*deg2rad;
00586 }
00587 
00588 
00589 /******************************
00590 // method Rob2Hoap
00591 //
00592 // Converts joint angles from  Robota rad referential
00593 // to Hoap deg referential
00594 
00595 ******************************/
00596 
00597 
00598 void Reaching::Rob2Hoap(pArmConfig_t conf)
00599 {
00600   conf->sfe = (-conf->sfe+pi/2) *rad2deg;
00601   conf->saa = (conf->saa-pi/2)*rad2deg;
00602   conf->shr = conf->shr*rad2deg;
00603   conf->eb = -conf->eb*rad2deg;
00604 }
00605 
00606 void Reaching::Rob2Hoap(joint_vec_t& conf)
00607 {
00608   conf[0] = (-conf[0]+pi/2) *rad2deg;
00609   conf[1] = (conf[1]-pi/2)*rad2deg;
00610   conf[2] = conf[2]*rad2deg;
00611   conf[3] = -conf[3]*rad2deg;
00612 }
00613 
00614 
00615 void Reaching::Rob2Icub(joint_vec_t& angles){
00616   angles[0] *= -rad2deg;   //sfe stays the same
00617   angles[1] = (-angles[1]+pi/2)*rad2deg;//saa
00618   angles[2] = (-angles[2]+pi/2)*rad2deg;//shr
00619   angles[3] = angles[3]*rad2deg; //eb
00620 }
00621 
00622 void Reaching::Icub2Rob(joint_vec_t& angles){
00623   angles[0] *= -deg2rad;   //sfe stays the same
00624   angles[1] = -angles[1]*deg2rad+pi/2;//saa
00625   angles[2] = -angles[2]*deg2rad+pi/2;//shr
00626   angles[3] = angles[3]*deg2rad; //eb
00627 }
00628 
00629 /***********************************
00630 // method Angle2Cart
00631 //
00632 // Performs the forward kinematic function
00633 // Input in Robota rad referential
00634 // Output in Eric's referential
00635 //
00636 ************************************/
00637 
00638 
00639 
00640 void Reaching::Angle2Cart(pArmConfig_t config, cart_vec_t& out){
00641   joint_vec_t& angle;
00642   v4_set(config->sfe,config->saa,config->shr,config->eb,angle);
00643   body->Angle2Cart(angle,out);
00644 }
00645 
00646 
00647 
00648 void Reaching::Angle2Cart(joint_vec_t& angle, cart_vec_t& out)
00649 {
00650   body->Angle2Cart(angle,out);
00651 }
00652 
00653 
00654 
00655 void Reaching::Angle2CartAbs(joint_vec_t& angle, cart_vec_t& out)
00656 {
00657   cart_vec_t& local;
00658   Angle2Cart(angle,local);
00659   Local2AbsRef(local,out); 
00660 }
00661 
00662 void Reaching::ElbowPosition(joint_vec_t& angle, cart_vec_t& out)
00663 {
00664   body->ElbowPosition(angle,out);
00665 }
00666 
00667 
00668 /*-------------------------------------------------
00669  * method GetAngle
00670  *------------------------------------------------*/
00677 void Reaching::GetAngle(joint_vec_t& angle) const {
00678   v4_copy(pos_angle,angle);
00679 }
00680 void Reaching::GetTargetAngle(joint_vec_t& angle) const {
00681   v4_copy(tar_angle,angle);
00682 }
00683 /*-------------------------------------------------
00684  * method GetAngle
00685  *------------------------------------------------*/
00692 // joint_vec_t& Reaching::GetAngle(){
00693 //   return pos_angle;
00694 // }
00695 
00696 /*-------------------------------------------------
00697  * method GetAbsHandPosition
00698  *------------------------------------------------*/
00705 void Reaching::GetAbsHandPosition(cart_vec_t& out) const {
00706   Local2AbsRef(pos_cart,out);
00707 }
00708 
00709 
00710 
00711 void Reaching::GetLocalHandPosition(cart_vec_t& out) const {
00712   v_copy(pos_cart,out);
00713 }
00714 
00715 
00716 /*****************************************
00717 // method InvKinematics
00718 //
00719 // Inverse kinematics method based on florent's
00720 // method. If the target is too far, the method will return
00721 // a position that points to the target (will reach a point on
00722 // the shoulder-target axis
00723 //
00724 // args:
00725 // relTar : target (in cartesian coordinates , eric's referential, origin at shoulder)
00726 // alpha : angle with the vertical (parameter of the solution curve)
00727 // out: where the solution is put
00728 //
00729 // returns:
00730 // 1. if the target is reachable or pointable while keeping in the joint angle bounds
00731 // 0. Otherwise.
00732 
00733 // Note: this method is not the cleanest, could be implemented using
00734 // quaternions (cf Hestenes)
00735 
00736 ****************************************/
00737 
00738 
00739 
00740 
00741 int  Reaching::InvKinematics (cart_vec_t& relTar, float alpha, float toler,  pArmConfig_t out){
00742   cart_vec_t& v;
00743   int ret;
00744   ret=body->InvKinematics(relTar,alpha,toler,v);
00745   out->FromVector(v);
00746   return ret;
00747 }
00748 
00749 /*******************************************
00750 // method FindNearestPosition
00751 //
00752 // Finds the nearest joint angle position to <pos1> out of
00753 // the set of positions that are in the configManifold
00754 // Closness is measured using the euclidean distance.
00755 // 
00756 // args:
00757 // @param pos1 the distance to which the candidates are evaluated
00758    @param out where the result is put 
00759 //
00760 // @return the squared euclidean distance to target
00761 //
00762 // Note: One could have a distance measure which takes into account
00763 // the physical world (energy, work, ...)
00764 // This is a brute force method (trying all possibilities). One could
00765 // think of something smarter if one has an inverse kinematics function
00766 ********************************************/
00767 
00768 
00769 float Reaching::FindNearestPosition(pArmConfig_t pos1,pArmConfig_t out)
00770 {
00771   ArmConfigList_t::iterator it;
00772   pArmConfig_t min;
00773   float mindist,dist;
00774 
00775   //  cout<<"manifold size: "<< configManifold.size()<<endl;
00776   mindist = AngleDistance(pos1,&(configManifold[0]));
00777   min = &(configManifold[0]);
00778   for(it=configManifold.begin();it!=configManifold.end();it++){
00779     dist = AngleDistance(pos1,&(*it)); 
00780     if(dist<mindist){
00781       mindist = dist;
00782       min = &(*it);
00783     }
00784   }
00785   out->sfe = min->sfe;
00786   out->saa = min->saa;
00787   out->shr = min->shr;
00788   out->eb = min->eb;
00789   return mindist;
00790 }
00791 
00792 /* for later
00793 float Reaching::FindNearestPosition(joint_vec_t& pos1,joint_vec_t& out)
00794 {
00795   ArmConfigList_t::iterator it;
00796   pArmConfig_t min;
00797   float mindist,dist;
00798 
00799   mindist = AngleDistance(pos1,&(configManifold[0]));
00800   min = &(configManifold[0]);
00801   for(it=configManifold.begin();it!=configManifold.end();it++){
00802     dist = AngleDistance(pos1,&(*it)); 
00803     if(dist<mindist){
00804       mindist = dist;
00805       min = &(*it);
00806     }
00807   }
00808   out.sfe = min->sfe;
00809   out.saa = min->saa;
00810   out.shr = min->shr;
00811   out.eb = min->eb;
00812   return mindist;
00813 }
00814 */
00815 /*************************************
00816 // method AngleDistance
00817 //
00818 // return squared euclidean distance between two
00819 // joint angle configurations
00820 ***************************************/
00821 
00822 float Reaching::AngleDistance(pArmConfig_t pos1,pArmConfig_t pos2)
00823 {
00824   float sum = 0.0;
00825 
00826   sum += (pos1->sfe - pos2->sfe)*(pos1->sfe - pos2->sfe);
00827   sum += (pos1->saa - pos2->saa)*(pos1->saa - pos2->saa);
00828 #ifndef UPPER_ARM_PRIORITY
00829   sum += (pos1->shr - pos2->shr)*(pos1->shr - pos2->shr);
00830   sum += (pos1->eb  - pos2->eb) * (pos1->eb  - pos2->eb);
00831 #endif
00832   return sum;
00833 }
00834 
00835 
00836 float Reaching::AngleDistance(joint_vec_t& pos1,joint_vec_t& pos2)
00837 {
00838   float sum = 0.0;
00839   int ind, i;
00840 #ifdef UPPER_ARM_PRIORITY
00841   ind = 2;
00842 #else
00843   ind = 4;
00844 #endif
00845   for(i=0;i<ind;i++){
00846     sum += (pos1[i]-pos2[i])*(pos1[i]-pos2[i]);
00847   }
00848   return sum;
00849 }
00850 
00851 /* for later
00852 float Reaching::AngleDistance(joint_vec_t& pos1,joint_vec_t& pos2)
00853 {
00854   joint_vec_t&  diff;
00855   v4_sub(pos1,pos2,diff);
00856   return v4_squ_length(diff);
00857 }
00858 */
00859 
00860 
00861 
00862 
00863 int Reaching::ArmConfig2CVector(pArmConfig_t arm,joint_vec_t& out){
00864   if(arm){
00865   out[0] = arm->sfe;
00866   out[1] = arm->saa;
00867   out[2] = arm->shr;
00868   out[3] = arm->eb;
00869   return 1;
00870   }
00871   else return 0;
00872 }
00873 
00874 
00875 int Reaching::CVector2ArmConfig(joint_vec_t& v, pArmConfig_t arm){
00876   if(arm){
00877    arm->sfe = v[0];
00878    arm->saa= v[1];
00879    arm->shr= v[2];
00880    arm->eb= v[3];
00881   return 1;
00882   }
00883   else return 0;
00884 }
00885 
00886 
00887 /***************************************************
00888  * method Jacobian
00889  *
00890  * valid for eric's referential and robota rad ref. 
00891  * args:
00892  * --- 
00893  * @param v the angle position at which to compute
00894  * the Jacobian matrix
00895  * @param jac  a 4x4 matrix filled up by the method
00896  * the last row contains zeros and 1 on the diagonal.
00897  * 
00898  * indices are 0 1 2 3 4 \\ 5 6 7 8 \\ ...    
00899 ***********************************************/
00900 void Reaching::Jacobian(joint_vec_t& v, CMatrix4_t jac){
00901   body->Jacobian(v,jac);
00902 }
00903 
00904 
00905 
00906 /***************************************************
00907  * method IntermediateJacobian
00908  *
00909  * computes the Jacobian for a point on the arm.
00910  * valid for eric's referential and robota rad ref. 
00911  * args:
00912  * --- 
00913  * @param link 1 for the upper arm, 2 for the foremarm
00914  * @param len length of that link
00915  * @param v the angle position at which to compute
00916  * the Jacobian matrix
00917  * @param jac  a 4x4 matrix filled up by the method
00918  * the last row contains zeros and 1 on the diagonal.
00919  *     
00920 ***********************************************/
00921 
00922 
00923 void Reaching::IntermediateJacobian(int link, float len,joint_vec_t& v, CMatrix4_t jac){
00924   body->IntermediateJacobian(link,len,v,jac);
00925 }
00926 
00927 
00928 void Reaching::SetAnglesInRange(joint_vec_t& angle){
00929   body->SetAnglesInRange(angle);
00930 }
00931 
00932 
00933 void Reaching:: UpdateWeights(){   // -- to modify for visual servo
00934 #ifdef BODY_SCHEMA
00935   joint_vec_t& min_angle, max_angle;
00936   body->GetAnglesLowerBound(min_angle);
00937   body->GetAnglesUpperBound(max_angle);
00938 #endif
00939 
00940 #ifdef DYN_WEIGHTS
00941 #ifdef SAI_WEIGHT_MODULATION
00942   float sai = SpuriousAttractorIndex();
00943 #endif
00944   for(int i=0;i<4;i++){
00945 
00946     weight_angle[i] = 0.5*base_weight_angle[i]*//
00947       (cos((pos_angle[i] - min_angle[i])*2*pi/
00948            (max_angle[i]-min_angle[i])+pi)+1);
00949 
00950 
00951 #ifdef DIST_WEIGHT_MODULATION
00952  weight_angle[i] *= (1.0-abs(tar_angle[i]-pos_angle[i])/(max_angle[i]-min_angle[i]));
00953 #endif
00954 #ifdef SAI_WEIGHT_MODULATION
00955     weight_angle[i] *= min(2,sai);
00956 #endif
00957   }
00958 #ifdef END_CART_WEIGHT
00959   cart_vec_t& tmp;
00960   v_sub(target,pos_cart,tmp);    
00961   float cart_dist = max(v_length(tmp)/150,1e-10);
00962   for(int i=0;i<4;i++){
00963     weight_angle[i] /= cart_dist;
00964   }
00965   cout<<"dist "<< cart_dist*150<<" weights ";coutvec4(weight_angle);
00966 #endif
00967 
00968 
00969 #endif
00970 
00971 }
00972 
00973 
00974 
00975 
00976 
00977 
00978 
00979 
00980 
00981 
00982 /**************************************************
00983  * method SpuriousAttractorIndex
00984  *
00985  * @return a number indicating how far we are from a
00986  * spurious attractor
00987  * 
00988  ****************************************************/
00989 float Reaching::SpuriousAttractorIndex()
00990 {
00991   joint_vec_t& da,tmp14,tmp24;
00992   cart_vec_t& dx;
00993   CMatrix4_t jac; //jacobian matrix
00994   cart_vec_t& wxm1,tmp13;
00995   float dalength;
00996 
00997   v_sub(target,pos_cart,dx);
00998   v4_sub(tar_angle,pos_angle,da);
00999  
01000  
01001   Jacobian(pos_angle,jac);
01002   inverse_diag3(weight_cart,wxm1);
01003   m3_diag_v_multiply(wxm1,dx,tmp13);
01004   m3_4_t_v_multiply(jac,tmp13,tmp14);
01005   m4_diag_v_multiply(weight_angle,tmp14,tmp24);
01006   v4_add(da,tmp24,tmp14);
01007   
01008   dalength = v4_length(da);
01009   if (dalength < 0.00001){
01010     return 2;
01011     }
01012   else{
01013     return (v4_length(tmp14))/dalength;
01014   }
01015 }
01016 
01017 #ifndef LIGHT_VERSION
01018 // returns hoap angles in rad
01019 void Reaching::RetrieveJointAngles(pHoap2_JointAngles angles)
01020 {
01021   joint_vec_t& v1,v2;
01022   //  Hoap2_JointAngles *angles;
01023   // angles =(Hoap2_JointAngles *)an;
01024   v4_copy(pos_angle,v1);
01025   Rob2Hoap(v1);
01026   v4_scale(v1,deg2rad,v2);
01027   angles->R_SFE = v2[0];
01028   angles->R_SAA = v2[1];
01029   angles->R_SHR = v2[2];
01030   angles->R_EB =  v2[3];
01031 }
01032 
01033 
01034 // angles given in Hoap deg referential
01035 int Reaching::SetActualHoapPosition(pHoap2_JointAngles angles){
01036   joint_vec_t& v;
01037   v[0] = angles->R_SFE;
01038   v[1] = angles->R_SAA;
01039   v[2] = angles->R_SHR;
01040   v[3] = angles->R_EB;
01041 //ArmConfig2CVector(angles,v);
01042   Hoap2Rob(v);
01043   return SetActualRobPosition(v);
01044 }
01045 #endif
01046 
01047 
01048 #ifdef TRACE_COSTS
01049 
01050 // works for homogeneous weights only. not clean
01051 float Reaching::GetCosts(joint_vec_t& des_angle,cart_vec_t& des_pos){
01052   joint_vec_t& v1,v2;
01053   cart_vec_t& w1,w2;
01054   float s1,s2;
01055   v4_sub(des_angle,pos_angle,v1);
01056   //m4_diag_v_multiply(weight_angle,v1,v2);
01057   if (weight_angle[0] == 0){
01058     s1 = v4_dot(v1,v1);
01059     return s1;
01060   }
01061   else{
01062     v4_scale(v1,1/weight_angle[0],v2);
01063     s1 = v4_dot(v1,v2);
01064   }
01065   v_sub(des_cart,pos_cart,w1);
01066   
01067   if (weight_cart[0] == 0){
01068     s2 = v_dot(w1,w1);
01069     return s2;
01070   }
01071   else{
01072      v4_scale(w1,1/weight_cart[0],w2);
01073      s2 = v_dot(w1,w2);
01074   }
01075 
01076   //  m3_diag_v_multiply(weight_cart,w1,w2);
01077  
01078   return (s1+s2)/(1/weight_cart[0]+1/weight_angle[0]);
01079 }
01080 
01081 #endif
01082 
01083 
01084 /*------------------------------------------------
01085  * method SetActualRobPosition
01086  *-----------------------------------------------*/
01094 int Reaching::SetActualRobPosition(joint_vec_t& angles){
01095   if(AcceptablePosition(angles)){
01096     v4_copy(angles,pos_angle);
01097     v4_set(0,0,0,0,v_angle);
01098     v_set(0,0,0,v_cart);
01099     Angle2Cart(pos_angle,pos_cart);
01100     CVector2ArmConfig(pos_angle,&position);
01101     v_clear(tar_angle);
01102     return 1;
01103   }
01104   else{
01105     return 0;
01106   }
01107 } 
01108 
01109 
01110 /*------------------------------------------------
01111  * method SetActualRobPosAndSpeed
01112  *-----------------------------------------------*/
01120 int Reaching::SetActualRobPosAndSpeed(joint_vec_t& position, joint_vec_t& speed){
01121   CMatrix4_t jac;
01122   if(SetActualRobPosition(position)){
01123     v4_copy(speed,v_angle);
01124     Jacobian(pos_angle,jac);
01125     m3_4_v_multiply(jac,speed,v_cart);
01126    return 1;
01127   }
01128   else{
01129     return 0;
01130   }
01131 }
01132 
01133 /*------------------------------------------------
01134  * method SetRobAcceleration
01135  *-----------------------------------------------*/
01142 int Reaching::SetRobAcceleration(joint_vec_t& acc){
01143   cart_vec_t& tmpc;
01144   joint_vec_t& tmpa;
01145   int ret =1;
01146   float tmp;
01147 #ifdef BODY_SCHEMA
01148   joint_vec_t& min_angle, max_angle;
01149   body->GetAnglesLowerBound(min_angle);
01150   body->GetAnglesUpperBound(max_angle);
01151 #endif
01152 
01153 
01154   v4_copy(pos_angle,tmpa);
01155   v4_add(v_angle,acc,v_angle);
01156   v4_add(pos_angle,v_angle,pos_angle);
01157   for (int i=0;i<4;i++){
01158     tmp = min(max_angle[i],max(min_angle[i],pos_angle[i]));
01159     if(tmp !=pos_angle[i]){
01160       ret = 0;
01161       pos_angle[i] = tmp;
01162     }
01163   }
01164   if (!ret){
01165     v4_sub(pos_angle,tmpa,v_angle);
01166   }
01167   v_copy(pos_cart,tmpc);
01168   Angle2Cart(pos_angle,pos_cart);
01169   v_sub(pos_cart,tmpc,v_cart);
01170   return ret;
01171 }
01172 
01173 
01174 
01175 // int Reaching::InitOutputStream(string& filename, ofstream **ostr){
01176 //   *ostr = new ofstream(filename);
01177 //   if (*ostr->fail()){
01178 //     return 0;
01179 //   }
01180 //   else{
01181 //     return 1;
01182 //   }
01183 // }
01184 
01185 
01186 /******************************************************************
01187 // method SaveConfig
01188 //
01189 // writes the arm configuration and target location in
01190 // a file. Format: "x y z: th1 th2 th3 th4" (x,y,z = target location)
01191 // args: <fname> the filename where to write the arm configuration
01192 // and target location (in shoulder-centered, erics referential)
01193 // 
01194 // 
01195 // returns: 1 if sucessful writing, 0 otherwise
01196 ******************************************************************/
01197 
01198 int Reaching::SaveConfig(string *fname)
01199 {
01200   if (!out_str){
01201     out_str = new ofstream(fname->c_str());
01202     if (out_str->fail()) {
01203       free(out_str);
01204       out_str = NULL;
01205       return 0;
01206     }
01207   }
01208   *out_str << target[0] <<" "<<target[1] <<" "<< target[2] << ":"<< *this<<endl;
01209 #ifdef BODY_SCHEMA
01210   *out_str << *body;
01211 #endif
01212   return 1;
01213 }
01214 
01215 
01216 
01217 int Reaching::RecordTraj_ACT(joint_vec_t& initpos, joint_vec_t& targ, string& fname)
01218 {
01219   ofstream out(fname.c_str());
01220   if(!out.fail()){
01221     if(SetArmConfigurationTarget(targ)){
01222       return StartRecordTraj(initpos,out);
01223     }
01224     return -2; //unvalid target position
01225   }
01226   return -3; //unvalid filename
01227 }
01228 
01229 
01230 int Reaching::RecordTraj(joint_vec_t& initpos, cart_vec_t& targ, string& fname)
01231 {
01232   ofstream out(fname.c_str());
01233    if(!out.fail()){
01234     if(SetTarget(targ)){
01235       return StartRecordTraj(initpos,out);
01236     }
01237     return -2; //unvalid target position
01238   }
01239   return -3; //unvalid filename
01240 }
01241       
01242 
01243 
01244 /******************************************
01245  *
01246  * method StartRecordTraj
01247  *
01248  * returns:
01249  * ------- 
01250  * 0 if target reached
01251  * 1 if target not reached
01252  * -1 if initial position not valid
01253  ******************************************/
01254 
01255 
01256 int Reaching::StartRecordTraj(joint_vec_t& initpos,ofstream& out){
01257   if(SetActualRobPosition(initpos)){
01258     out << *this << endl;
01259     for (int i=0;i<MAX_IT && !TargetReached();i++){
01260       ReachingStep();
01261       out << *this << endl;
01262     }
01263     out.close();
01264     if(TargetReached()){
01265       return 0; // everything ok;
01266     }
01267     else{
01268       return 1;
01269     }
01270   } 
01271   return -1; //unvalid init position
01272 }
01273 
01274 int Reaching::StartRecordTraj(joint_vec_t& initpos){
01275   if(SetActualRobPosition(initpos)){
01276     return Reach();
01277   }
01278   else{
01279     return -1; //unvalid init position
01280   }
01281 } 
01282 
01283 int Reaching::Reach(){
01284   for (int i=0;i<MAX_IT && !TargetReached();i++){
01285     ReachingStep();
01286   }
01287   if(TargetReached()){
01288     return 0; // everything ok;
01289   }
01290   else{
01291     return 1;
01292   }
01293 } 
01294 
01295 
01296 
01297 /**********************************************
01298 // method AcceptablePosition
01299 //
01300 // returns: 
01301 // 1 if <angle> is within workspace boundaries
01302 // 0 otherwise. 
01303 ***********************************************/
01304 
01305 // angles must be given in rad robota referential
01306 #ifndef BODY_SCHEMA
01307 int Reaching::AcceptablePosition(joint_vec_t& angle){
01308   int i;
01309   for(i=0;i<4;i++){
01310     //if (max(min_angle[i], min(max_angle[i],angle[i])) != angle[i]){
01311     if(min_angle[i]>=angle[i] || max_angle[i]<=angle[i]){
01312 #ifndef SILENT
01313       //      cout << "angle " <<i <<" "<<angle[i]<<" beyond limits"<<endl;
01314 #endif
01315       return 0;
01316     }
01317   }
01318   return 1;
01319 }
01320 #else
01321 int Reaching::AcceptablePosition(joint_vec_t& angle){
01322   return body->AcceptablePosition(angle);
01323 }
01324 #endif
01325 /*************************************************
01326  * method TargetReached
01327  * 
01328  * Target is considered to be reached if within a distance of <tol>
01329  * from end-effector and the arm is not moving anymore
01330  *
01331  * returns: 1 if target is reached
01332  *          0 otherwise
01333  *************************************************/
01334 
01335 inline int Reaching::TargetReached(){
01336   cart_vec_t& dist;
01337   float d;
01338   v_sub(target,pos_cart,dist);
01339   d = v_length(dist);
01340   return d<tol && HasStopped();
01341 }
01342 
01343 inline int Reaching::HasStopped(){
01344   return v4_length(v_angle)<zero_vel_angle;
01345 }
01346 
01347 
01348 /**********************************************************************
01349 
01350 Some useful methods to perform tests and simulations
01351 
01352 ***********************************************************************/
01353 #ifndef BODY_SCHEMA
01354 void Reaching::RandomAnglePos(joint_vec_t& angle){
01355   float rn;
01356   for (int i=0;i<4;i++){
01357     rn = abs(rand()/((float)(RAND_MAX+1)));
01358     angle[i] = min_angle[i]+(max_angle[i]-min_angle[i])*rn; 
01359   }
01360 }
01361 #else
01362 void Reaching::RandomAnglePos(joint_vec_t& angle){
01363   body->RandomAnglePos(angle);
01364 }
01365 #endif
01366 
01371 #ifndef BODY_SCHEMA
01372 void Reaching::RandomCartPos(cart_vec_t& cart){
01373   joint_vec_t& angle;
01374   RandomAnglePos(angle);
01375   Angle2Cart(angle,cart);
01376 }
01377 #else
01378 void Reaching::RandomCartPos(cart_vec_t& cart){
01379   body->RandomCartPos(cart);
01380 }
01381 #endif
01382 
01383 
01384 void Reaching::RandomCartAbsPos(cart_vec_t& cart){
01385   cart_vec_t& cart_l; //local ref
01386   RandomCartPos(cart_l);
01387   Local2AbsRef(cart_l,cart);
01388 }
01389 
01390 void Reaching::Local2AbsRef(const cart_vec_t& cart_l,cart_vec_t& cart) const{
01391   v_add(cart_l,shoulder_abs_pos,cart);
01392 }
01393 
01394 
01395 void Reaching::Abs2LocalRef(const cart_vec_t& cart, cart_vec_t& out) const {
01396   v_sub(cart,shoulder_abs_pos,out);
01397   //  cout <<"r reach"<<endl;
01398 }
01399 #ifndef BODY_SCHEMA
01400 int Reaching::GetVisualMode(){return 0;}
01401 #else
01402 int Reaching::GetVisualMode(){return body->GetVisualMode();}
01403 #endif
01404 
01405 // // target is already specified
01406 // void Reaching::VelocityConstrainedMovement(char *filename){
01407   
01408 //   int t;
01409 //   vector<> vel;
01410 //   ReadVelocityProfile(filename,&vel);
01411 //   len = vel.size()+delay;
01412 //   for(t=0;t<len;t++){
01413 //     ReachingStep();
01414     
01415 
01416 
01417 
01418 #ifdef MANY_JUMPS
01419 
01420 int Reaching::PerformTrajectories(string *infname, string *outfname){
01421 
01422     ifstream istr(infname->c_str());
01423 
01424   if(istr.fail()){
01425     cout <<"can't open file "<<infname->c_str()<<endl;
01426     return 0;
01427   }
01428   else{
01429     int timeOfJump, tar_ok,tar_spec;
01430     string fname(*outfname);
01431     string line;
01432     char nc[80];
01433     joint_vec_t& pos, new_tar;
01434     int n=0;
01435 
01436     sprintf(nc,"%d",n);
01437     fname = *outfname  + nc +".txt"; // appending the traj number to the filename
01438     ofstream out(fname.c_str());
01439     getline(istr,line);
01440     if(4 != sscanf(line.c_str(),"%f %f %f %f \n", &(pos[0]),&(pos[1]),&(pos[2]),&(pos[3]))){
01441 #ifndef SILENT
01442         cout << "line " << n << " not decripted"<<endl;
01443 #endif
01444     }
01445     else{
01446       SetActualRobPosition(pos);
01447     }
01448     out << *this << endl; 
01449     while(!istr.eof()){
01450       getline(istr,line);
01451       tar_spec = sscanf(line.c_str(),"%d: %f %f %f %f\n", 
01452                              &timeOfJump, &(new_tar[0]),&(new_tar[1]),&(new_tar[2]), &(new_tar[3]));
01453       if ((tar_spec !=5) && (tar_spec != 4)){
01454 #ifndef SILENT
01455         //cout << "line " << n << " not decripted"<<endl;
01456 #endif
01457       }
01458       else{
01459         if (tar_spec ==4){                        // a 3d cartesian end-effector is specified   
01460           tar_ok = SetTarget(new_tar);            // only the first 3 values
01461         }
01462         else{
01463           tar_ok = SetArmConfigurationTarget(new_tar); // a 4d arm configuration is specified
01464           //      coutvec4(new_tar);
01465         }
01466         if(tar_ok){
01467           for (int i=0;i<timeOfJump && !TargetReached();i++){
01468             ReachingStep();
01469 #ifdef LEARNING
01470             LearningLocal();
01471 #endif
01472             out << *this << endl;
01473           }
01474 #ifdef LEARNING
01475           LearningGlobal();
01476 #endif
01477         }
01478       } 
01479     }
01480     out.close();
01481     if(TargetReached()){
01482       return 1;
01483     }
01484   }
01485   return 0;
01486 }
01487 #endif
01488 
01489 
01490 
01491 
01492 #ifdef TARGET_JUMP
01493 
01494 int Reaching::PerformTrajectories(string *infname, string *outfname){
01495   int n =1;
01496   
01497   ifstream istr(infname->c_str());
01498 
01499   if(istr.fail()){
01500     cout <<"can't open file "<<infname->c_str()<<endl;
01501     return 0;
01502   }
01503   else{
01504     int timeOfJump;
01505     string fname(*outfname);
01506     string line;
01507     char nc[80];
01508     cart_vec_t& tar, new_tar;
01509     joint_vec_t& pos;
01510     while(!istr.eof()){
01511       sprintf(nc,"%d",n);
01512       fname = *outfname  + nc +".txt";
01513       getline(istr,line);
01514       if(11 != sscanf(line.c_str(),"%f %f %f:%f %f %f %f:%d:%f %f %f ",&(tar[0]),&(tar[1]),&(tar[2]),
01515                      &(pos[0]),&(pos[1]),&(pos[2]),&(pos[3]), &timeOfJump, &(new_tar[0]),&(new_tar[1]),&(new_tar[2]))){
01516 #ifndef SILENT
01517         cout << "line " << n << " not decripted"<<endl;
01518 #endif
01519       }
01520       else{
01521         RecordTraj(pos,tar,fname,timeOfJump,new_tar);
01522       }
01523       n++;
01524     }
01525   }
01526   return n;
01527 }
01528 
01529 #else
01530 #ifndef MANY_JUMPS
01531 
01532 
01533 // returns the  number of successful reaching movements
01534 int Reaching::PerformTrajectories(string *infname, string *outfname,int max_traj){
01535   int n =1;
01536   int cnt_reached = 0; //counts the number of successful movement
01537   ifstream istr(infname->c_str());
01538 
01539   if(istr.fail()){
01540     cout <<"can't open file "<<infname->c_str()<<endl;
01541     return 0;
01542   }
01543   else{
01544     string fname(*outfname);
01545     string line;
01546     char nc[80];
01547     cart_vec_t& tar;
01548     joint_vec_t& pos;
01549     if(max_traj<0){max_traj = INT_MAX;}
01550     while(!istr.eof() && n<=max_traj){
01551       sprintf(nc,"%d",n);
01552       fname = *outfname  + nc +".txt";
01553       getline(istr,line);
01554       if(7 != sscanf(line.c_str(),"%f %f %f : %f %f %f %f ",&(tar[0]),&(tar[1]),&(tar[2]),
01555                      &(pos[0]),&(pos[1]),&(pos[2]),&(pos[3]))){
01556 #ifndef SILENT
01557         cout << "line " << n << " not decripted"<<endl;
01558 #endif
01559       }
01560       else{
01561         if(!RecordTraj(pos,tar,fname)){
01562           cnt_reached++;
01563         }
01564       }
01565       n++;
01566     }
01567   }
01568   return cnt_reached;
01569 }
01570 
01571 #endif
01572 #endif
01573 
01574 /*--------------------------------------------
01575  * method PerformPerturbedTrajectory
01576  *-------------------------------------------*/
01585 int Reaching::PerformPerturbedTrajectory(string *infname, string *outfname){
01586   string line;
01587   int i,perturbationTime,n=1;
01588   cart_vec_t& tar;
01589   joint_vec_t& pos,acc;
01590   ifstream istr(infname->c_str());
01591   string fname(*outfname);
01592 
01593   if(istr.fail()){
01594     cout <<"can't open file "<<infname->c_str()<<endl;
01595     return 0;
01596   }
01597   else{
01598     getline(istr,line);
01599     if(7 != sscanf(line.c_str(),"%f %f %f : %f %f %f %f ",&(tar[0]),&(tar[1]),&(tar[2]),
01600                      &(pos[0]),&(pos[1]),&(pos[2]),&(pos[3]))){
01601 #ifndef SILENT
01602       cout << "line " << n << " not decripted"<<endl;
01603 #endif
01604       return -3;
01605     }
01606     else{
01607       if(SetTarget(tar)){
01608         if(SetActualRobPosition(pos)){
01609           ofstream out(fname.c_str());
01610           while(!istr.eof()){ 
01611             n++;
01612             getline(istr,line);
01613             if(5!= sscanf(line.c_str(),"%d : %f %f %f %f",&perturbationTime, 
01614                           &(acc[0]),&(acc[1]), &(acc[2]),&(acc[3]))){
01615 #ifndef SILENT
01616               cout << "line " << n << " not decripted"<<endl;
01617 #endif
01618             }
01619             else{
01620               for(i=1;i<perturbationTime;i++){
01621                 ReachingStep();
01622                 out << *this << endl;
01623               }
01624               SetRobAcceleration(acc);
01625               out << *this << endl;
01626             }
01627           }
01628           for(i=0;i<MAX_IT && !TargetReached();i++){
01629             ReachingStep();
01630             out << *this << endl;
01631           }
01632           out.close();
01633           return TargetReached();
01634         }
01635         else{
01636           return -1;
01637         }
01638       }
01639       else{
01640         return -2;
01641       }
01642     }
01643   }
01644 }
01645 
01646 
01647 #ifdef WITH_ENVIRONMENT
01648 
01649 int Reaching::SetEnvironment(Environment *e){
01650   if(!env){
01651     env = e;
01652     return 1;
01653   }
01654   else{
01655     return 0;
01656   }
01657 }
01658 
01659 #endif
01660 
01661 #ifdef OBSTACLE_AVOIDANCE
01662 int Reaching::LinkDistanceToObject(int link, EnvironmentObject *obj, float *dist, 
01663                          float *point, cart_vec_t& contact_vector){
01664   cart_vec_t& objPos;
01665   cart_vec_t& lowerLinkExtr;
01666   cart_vec_t& upperLinkExtr;
01667   cart_vec_t& v1,v2,linkv,u,vertexPos,v_tmp,tmp3;
01668   CMatrix4_t ref,tmpmat,tmpmat2;
01669   cart_vec_t& s1,s2;
01670   int i,j,k,dir1,dir2,pindex,min_i;
01671   float alldists[12]; //closest distance to each edge
01672   float allpoints[24]; // closest pair of points on each edge
01673   float min_dist;
01674   int s3[3];
01675 
01676   for (i=0;i<12;i++){
01677     alldists[i]=FLT_MAX;
01678   }
01679 
01680   switch(link){
01681   case 1:
01682     v_clear(upperLinkExtr);
01683     ElbowPosition(pos_angle,lowerLinkExtr);
01684     break;
01685   case 2:
01686     ElbowPosition(pos_angle,upperLinkExtr);
01687     v_copy(pos_cart,lowerLinkExtr);
01688     break;
01689   default:
01690     cout<<"unvalid link number"<< endl;
01691   }
01692   if(!obj){
01693     return 0;
01694   }
01695   //  v_sub(obj->solid->m_position, shoulder_abs_pos,objPos);
01696     Abs2LocalRef(obj->GetPosition(), objPos);
01697   //  cout<<"ule: ";coutvec(upperLinkExtr);
01698   //  cout<<"lle: ";coutvec(lowerLinkExtr);
01699     //  coutvec(objPos);
01700   v_sub(lowerLinkExtr,objPos,v1);
01701   v_sub(upperLinkExtr,objPos,v2);
01702   //m_transpose(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
01703   m_copy(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
01704   v_clear(s3);
01705   
01706   //checking when two parallel sides must me checked
01707   for(i=0;i<3;i++){
01708     s1[i] = v_dot(v1, &ref[i*4]);
01709     s2[i] = v_dot(v2, &ref[i*4]);
01710     if (s1[i]*s2[i]>=0){
01711       s3[i] = sign(s1[i]+s2[i]);
01712     }
01713   }
01714   //    cout << "s3: ";coutvec(s3);
01715   m_copy(ref,tmpmat);
01716   for(i=0;i<3;i++){
01717     if(s3[i]){
01718       v_sub(lowerLinkExtr,upperLinkExtr,linkv);
01719       v_scale(obj->solid->m_size,-0.5,u);
01720       u[i] *=-s3[i];
01721       v_add(objPos,u,vertexPos);
01722       //      cout<<"vPos "<<i<<": ";coutvec(vertexPos);
01723       v_scale(&(ref[i*4]),s3[i],&(tmpmat[i*4]));
01724       // cout<<"norm "<<i<<": ";coutvec((tmpmat+i*4));
01725       m_inverse(tmpmat,tmpmat2);
01726       if(v_dot(&(tmpmat[i*4]),linkv)<0){
01727         v_sub(lowerLinkExtr,vertexPos,v_tmp);
01728         *point = 1;
01729       }
01730       else{
01731         v_sub(upperLinkExtr,vertexPos,v_tmp);
01732         *point = 0;
01733       }
01734       //      cout<<"linkv ";coutvec(linkv);
01735       // cout <<"pt "<<*point<<endl;
01736         
01737       
01738 // #ifdef OLD_AV
01739 //       v_copy(linkv,(cart_vec_t&)&tmpmat[i*4]);
01740 //       m_inverse(tmpmat,tmpmat2);
01741 //       v_sub(upperLinkExtr,vertexPos,tmp2);
01742       // tmp3 should contain the intersection coordinates in
01743       // the referential defined by the edges of the surface 
01744       v_transform_normal(v_tmp,tmpmat2,tmp3); 
01745       // cout<<"tmp3 ";coutvec(tmp3);
01746       if(tmp3[(i+1)%3]>=0 && tmp3[(i+2)%3]>=0 // the link points to the rectangle
01747          && tmp3[(i+1)%3]<=obj->solid->m_size[(i+1)%3] && 
01748          tmp3[(i+2)%3]<=obj->solid->m_size[(i+2)%3]){
01749         if(tmp3[i]<0){
01750           return 0; // there is a collision
01751         }
01752         else{
01753           *dist = tmp3[i];
01754           v_copy(&(tmpmat[i*4]),contact_vector);
01755           v_scale(contact_vector,*dist,contact_vector);
01756           return 1;
01757         }
01758       }
01759     }
01760   }
01761   // the link does not point to a rectangle -> look for the edges
01762   v_scale(obj->solid->m_size,-0.5,u);
01763   for(i=0;i<3;i++){// each kind of edge
01764       dir1 = s3[(i+1)%3]; 
01765       dir2 = s3[(i+2)%3];
01766     for(j=0;j<2;j++){ 
01767       if(dir1 == 0 || dir1==2*j-1){ //edges of this face must be computed
01768         for(k=0;k<2;k++){
01769           if(dir2 == 0 || dir2==2*k-1){ //edges of this face must be computed
01770             v_copy(u,v_tmp);
01771             v_tmp[(i+1)%3]*=-(2*j-1);
01772             v_tmp[(i+2)%3]*=-(2*k-1);
01773             v_add(objPos,v_tmp,vertexPos);
01774             v_scale(&(ref[4*i]),obj->solid->m_size[i],v1); // edge with the right length
01775             pindex = 4*i+2*j+k;
01776             FindSegmentsNearestPoints(vertexPos,v1,upperLinkExtr,linkv,&(allpoints[2*pindex]),&(allpoints[2*pindex+1]),&(alldists[pindex]));
01777         //     cout<<"i:"<<i<<" j:"<<j<<" k:"<<k<<"dist "<<alldists[pindex]<<endl;
01778 //          cout<<"v1:";coutvec(v1);
01779 //          cout<<"ref:";coutvec((&(ref[4*i])));
01780 //          cout<<"vP:";coutvec(vertexPos);
01781           }
01782         }
01783       }
01784     }
01785   }
01786   //looking for the min
01787   min_dist = alldists[0];
01788   min_i = 0;
01789   for(i=1;i<12;i++){
01790     if(alldists[i] < min_dist){
01791       min_dist = alldists[i];
01792       min_i = i;
01793     }
01794   }
01795   // returning the min distance
01796   *dist = min_dist;
01797   *point = allpoints[2*min_i+1];
01798   v_scale(linkv,*point,v1);
01799   v_add(upperLinkExtr,v1,v2); // nearest point on link
01800   k = min_i%2; //retrieving the right edge
01801   j = ((min_i-k)%4)/2;
01802   i = min_i/4; 
01803   //  cout<<"ijk: "<<i<<" "<<j<<" "<<k<<endl;
01804   v_copy(u,v_tmp);
01805   v_tmp[(i+1)%3]*=-(2*j-1);
01806   v_tmp[(i+2)%3]*=-(2*k-1); 
01807   v_add(objPos,v_tmp,vertexPos); // starting vertex of the edge
01808   v_scale(&(ref[3*1]),allpoints[2*min_i]*(obj->solid->m_size[min_i]),v1);
01809   v_add(vertexPos,v1,v1); // nearest point on solid
01810   v_sub(v2,v1,contact_vector);
01811   return 1;
01812 }
01813 
01814 //nearest points between two segments given by pi+veci, results given in ratio of vec [0 1]
01815 int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){
01816   CMatrix3_t mat;
01817   CMatrix3_t invmat;
01818   cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2;
01819   int i; 
01820   m_identity(mat);
01821   v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not //
01822   if(v_squ_length(vec3)<0.00001){
01823     return 0;
01824   }
01825 
01826   v_scale(vec2,-1,v2);
01827   m_set_v3_column(vec1,0,mat);
01828   m_set_v3_column(v2,1,mat);
01829   m_set_v3_column(vec3,2,mat);
01830   m_inverse(mat,invmat);
01831   v_sub(p2,p1,tmp);
01832   v_transform_normal(tmp,invmat,k);
01833   for(i=0;i<2;i++){
01834     k[i] = max(min(1,k[i]),0);
01835   }
01836   v_scale(vec1,k[0],tmp);
01837   v_add(p1,tmp,tmp1);
01838   v_scale(vec2,k[1],tmp);
01839   v_add(p2,tmp,tmp2);
01840   v_sub(tmp2,tmp1,tmp);
01841   *dist=v_length(tmp);
01842   *nearest_point1 = k[0];
01843   *nearest_point2 = k[1];
01844   return 1;
01845 }
01846 
01847   
01848       
01849 
01850             
01851 #endif
01852 #ifdef LEARNING
01853   float Reaching::LearningLocal(){return 0.0;}
01854   float Reaching::LearningGlobal(){return 0.0;}
01855 #endif
01856 
01857 /********************************************
01858 // operator <<
01859 //
01860 // overriding the << operator for the ostream class
01861 // used for cout << this
01862 **********************************************/
01863 
01864 ostream& operator<<(ostream& out, const Reaching& reach)
01865 {
01866 
01867 
01868 //   out << reach.pos_angle[0]<<" "<<reach.pos_angle[1]<<" "<<reach.pos_angle[2]
01869 //       <<" "<<reach.pos_angle[3] << " "<<reach.pos_cart[0]<<" "
01870 //       <<reach.pos_cart[1]<<" "<<reach.pos_cart[2];
01871   joint_vec_t& p_angle;
01872   cart_vec_t& p_cart;
01873   reach.GetAngle(p_angle);
01874   reach.GetAbsHandPosition(p_cart);
01875     
01876 //   out << p_angle[0]<<" "<<p_angle[1]<<" "<<p_angle[2]
01877 //       <<" "<<p_angle[3] << " "<<p_cart[0]<<" "
01878 //       <<p_cart[1]<<" "<<p_cart[2];
01879   out<<p_angle<<" "<<p_cart;
01880 #ifdef PRINT_WEIGHTS
01881   out << " "<< reach.weight_angle[0]<<" "<< reach.weight_angle[1]<<" "<< 
01882     reach.weight_angle[2]<<" "<< reach.weight_angle[3]<<" "<< 
01883     reach.weight_cart[0]<<" "<< reach.weight_cart[1]<<" "<<
01884     reach.weight_cart[2];
01885 #endif
01886 #ifdef PRINT_TARGET
01887 reach.GetTargetAngle(p_angle);  
01888  out<<" "<<p_angle;
01889 #endif
01890 #ifdef PRINT_SAI 
01891   float index = ((Reaching)reach).SpuriousAttractorIndex();
01892   out <<" "<< index;
01893 #endif
01894   return out;
01895 }
01896  
01897 istream& operator>>(istream& in, Reaching& r){
01898   cart_vec_t& tar;
01899   joint_vec_t& pos;
01900   char c;
01901   in>>tar[0]>>tar[1]>>tar[2]>>c>>pos[0]>>pos[1]>>pos[2]>>pos[3];
01902   if(!r.SetActualRobPosition(pos)){
01903     cout<<"initial position out of range"<<endl;
01904   }
01905   r.SetLocalTarget(tar);    
01906   return in;
01907 }
01908 
01909 
01910 
01911 ostream& operator<<(ostream& out, const ArmConfigList_t manifold){
01912   //  int i,n;
01913   ArmConfigList_t::const_iterator it;
01914   joint_vec_t& angles;
01915   for(it=manifold.begin();it!=manifold.end();it++){
01916     it->ToVector(angles);
01917     out<<angles<<endl;
01918   }
01919   return out;
01920 }
01921 
01922 #endif //NOT_YET
01923 
01924 
 All Data Structures Functions Variables

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