ReachingGen.h

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 #ifndef __REACHING_H__
00020 #define __REACHING_H__
00021 
00022 
00023 using namespace std;
00024 #include <iostream>
00025 #include <fstream>
00026 #include <string>
00027 #include <vector>
00028 #include <assert.h>
00029 
00030 
00031 
00032 //#define OBSTACLE_AVOIDANCE
00033 
00034 
00035 #define NEW_REACHING_STEP
00036 //#define SILENT
00037 
00038 
00039 #define ADAPT_VITE
00040 #ifndef M_DEBUG
00041 #define M_DEBUG
00042 #endif
00043 
00044 
00045 #define DYN_WEIGHTS
00046 
00047 
00048 //#define PRINT_WEIGHTS
00049 #define PRINT_TARGET
00050 
00051 //#define BODY_SCHEMA
00052 
00053 //#define MANY_JUMPS
00054 //#define LEARNING
00055 #define MAX_IT 500
00056 
00057 //#define TRACE_COSTS
00058 //#define REST_POS
00059 //#define ADAPTIVE_REST_POS
00060 
00061 /* #ifdef REST_POS */
00062 /* #define MAX_IT 200 */
00063 /* #endif */
00064 
00065 
00066 
00067 
00068 
00069 /* #ifdef OBSTACLE_AVOIDANCE */
00070 /* #define WITH_ENVIRONMENT */
00071 /* #endif */
00072 
00073 
00074 /* #ifdef WITH_ENVIRONMENT//OBSTACLE_AVOIDANCE */
00075 /* class Reaching; */
00076 /* class ReachingRight; */
00077 /* class ReachingLeft; */
00078 /* #include "Environment.h" */
00079 /* #endif */
00080 
00081 
00082 
00083 #include "BodySchema.h"
00084 
00085 
00086 
00095 class Reaching
00096 {
00097 
00098  public:
00099 
00100 
00101   BodySchema *body;
00102 
00103 
00104   cart_vec_t shoulder_abs_pos;    // shoulder position
00105 
00106   float incr;                     // configManifold coarseness
00107 
00108   cart_vec_t target;              // target  relative to shoulder position
00109   float tol;                      // zero distance to target 
00110     float zero_vel_angle;            // zero angular velocity
00111 
00112     int pure_joint_ctl;
00113   cart_vec_t weight_cart;         // inverse of cost function weights for angular values
00114   joint_vec_t weight_angle;        // inverse of cost function weights for cartesian values
00115 
00116 #ifdef DYN_WEIGHTS
00117   cart_vec_t base_weight_cart;         // base weights for angular values
00118   joint_vec_t base_weight_angle;        // base weights for cartesian values
00119 #endif
00120 
00121 #ifdef OBSTACLE_AVOIDANCE
00122 
00123   float obstacle_rad;
00124   float nu;
00125   joint_vec_t des_a_angle_obs; //acceleration du to obstacle potential
00126 #endif
00127 
00128 
00129  protected:
00130 
00131 
00132 
00133   // vite variables
00134   float alpha;
00135   float beta;
00136 
00140   joint_vec_t v_angle;   
00141 
00142 
00146   cart_vec_t v_cart;    // 
00147 
00148 
00152   joint_vec_t des_angle; // 
00153 
00154 
00158   cart_vec_t des_cart;  // 
00159 
00160 
00164   joint_vec_t pos_angle; // 
00165 
00166 
00170   cart_vec_t pos_cart;  //
00171 
00172 
00176   joint_vec_t tar_angle; //
00177 
00178 
00182   joint_vec_t des_v_angle; 
00183 
00184 
00188   cart_vec_t des_v_cart;    
00189  
00190 
00191   
00192   //input-output stuff
00193  
00197  ofstream *out_str; //for streaming out
00198   
00199 
00200  
00201  public:
00202 
00206     Reaching();
00207    
00211     virtual ~Reaching();
00212 
00216     void  SetBodySchema(BodySchema *b);
00217    
00222     BodySchema* GetBodySchema();
00223 
00224 
00229     void SetTargetAngle(joint_vec_t& angles);
00230 
00235     void GetTarget(cart_vec_t& pos){pos=target;};
00236 
00241     void GetTargetAngle(joint_vec_t& joint){joint=tar_angle;}
00242 
00250     int SetLocalTarget(cart_vec_t& new_target,int strict=0);
00251 
00252 
00258   int TargetReached(){return (target-pos_cart).Norm2() < 2*tol*tol && v_angle.Norm2()<zero_vel_angle*zero_vel_angle;}
00259   
00267   void ReachingStep(float dt=1);
00268 
00277   void ViteCart(cart_vec_t& target,cart_vec_t& pos,cart_vec_t& speed,
00278        cart_vec_t& new_pos, cart_vec_t& new_speed);
00279   
00288     void ViteAngle(joint_vec_t& target,joint_vec_t& pos,joint_vec_t& speed,
00289        joint_vec_t& new_pos, joint_vec_t& new_speed);
00290 
00299   void ProjectVector(cart_vec_t& v3,joint_vec_t& v4, joint_vec_t& out);
00300 
00301 
00307   void GetPosition(cart_vec_t& cart,joint_vec_t& joint){cart=pos_cart;joint=pos_angle;}
00308     
00313     void GetAnglePosition(joint_vec_t& joint){joint = pos_angle;}
00314  
00318     void MatchToBodySchema();
00319 
00324     void RandomTarget(int strict=1){cart_vec_t p;RandomTarget(p,strict);}
00325     
00326 
00332 void RandomTarget(cart_vec_t& tar,int strict=1);
00333   
00334 
00343   float UpdateLocalTarget(cart_vec_t *new_target = NULL);
00344 
00345 
00349     void SetStill();
00350     
00351 
00355     void PureJointControl(){pure_joint_ctl=1;};
00356 
00360     void HybridControl(){pure_joint_ctl=0;};
00361 
00365     void Print(){cout<<pos_cart<<" : "<<target<<" -  "<<pos_angle<<" : "<<tar_angle<<endl;}
00366 
00367  protected:
00368 
00369   void UpdateWeights();
00370 
00371 
00372 #ifdef NOT_YET
00373 
00374 
00375 
00376 
00388   virtual int ComputeConfigManifold(cart_vec_t& target);
00389   
00407 int InvKinematics (cart_vec_t& target, float alpha, float tol, pArmConfig_t out);
00408 
00409 
00415  void Icub2Rob(joint_vec_t& angles);
00416 
00417 
00423  void Rob2Icub(joint_vec_t& angles);
00424 
00425 
00431   void Hoap2Rob(pArmConfig_t conf);
00432 
00433 
00439   void Hoap2Rob(joint_vec_t& conf);
00440 
00445   void Rob2Hoap(pArmConfig_t conf);
00446 
00451   void Rob2Hoap(joint_vec_t& conf);
00452 
00458   virtual void Angle2Cart(pArmConfig_t config, cart_vec_t& out);
00459   
00460 
00466   virtual void Angle2Cart(joint_vec_t& config, cart_vec_t& out);
00467 
00473   void Angle2CartAbs(joint_vec_t& config, cart_vec_t& out);
00474 
00475   void ElbowPosition(joint_vec_t& config, cart_vec_t& out);
00476 
00482   void SetArmLength(float l1,float l2);
00483   void SetWeights(cart_vec_t& w_cart, joint_vec_t& w_angle);
00484   void SetIncrement(float new_increment);
00485 
00486   void DecrementAngleWeights(float factor=2.0);
00487   void IncrementAngleWeights(float factor=2.0);
00488   void IncrementCartWeights(float factor=2.0);// beware, not to set to 0.
00489 
00497   virtual int SetTarget(cart_vec_t& new_target,int strict=0);
00498 
00499 
00500   int SetArmConfigurationTarget(joint_vec_t& new_target);
00501 
00502   int UpdateTarget();
00503 
00504  
00519   float FindNearestPosition(pArmConfig_t pos1,pArmConfig_t out);
00520 
00529   float AngleDistance(pArmConfig_t pos1,pArmConfig_t pos2);
00538   float AngleDistance(joint_vec_t& pos1,joint_vec_t& pos2);
00539 
00548   virtual void Jacobian(joint_vec_t& angle, CMatrix4_t jac);
00549 
00565   void IntermediateJacobian(int link, float len,joint_vec_t& v, CMatrix4_t jac);
00566   int ArmConfig2CVector(pArmConfig_t arm,joint_vec_t& out);
00567   int CVector2ArmConfig(joint_vec_t& v,pArmConfig_t out);
00568 #ifdef TRACE_COSTS
00569   float GetCosts(joint_vec_t& des_angle,cart_vec_t& des_cart);
00570   float ReachingStep();
00571 #else
00572 
00581 #endif
00582 #ifndef REACH_MASTER
00583 #ifndef LIGHT_VERSION  
00584   void RetrieveJointAngles(pHoap2_JointAngles angles);
00585   int SetActualHoapPosition(pHoap2_JointAngles angles);
00586 #endif  
00587 #endif
00588   
00589   
00590 
00596   int SetActualRobPosition(joint_vec_t& angles);
00597 
00598 
00605   int SetActualRobPosAndSpeed(joint_vec_t& position, joint_vec_t& speed);
00606 
00612   int SetRobAcceleration(joint_vec_t& angle_acc);
00613   
00622   void SetShoulderPosition(cart_vec_t& position);
00623   
00630   int AcceptablePosition(joint_vec_t& angle);
00631 
00639   int SaveConfig(string *fname);
00640   int InitOutputStream(string& filename, ofstream **ostr);
00641   
00652   int PerformTrajectories(string *infname, string *outfname, int max_traj=-1);
00653 
00664   int PerformPerturbedTrajectory(string *infname, string *outfname);
00665   int RecordTraj(joint_vec_t& initpos, cart_vec_t& target,string& fname);
00666   int RecordTraj_ACT(joint_vec_t& initpos, joint_vec_t& target,string& fname);
00667 
00676   int StartRecordTraj(joint_vec_t& initpos,ofstream& out); 
00677 
00685   int StartRecordTraj(joint_vec_t& initpos); // no trajectory output
00686  
00692   int Reach();
00693 
00702   int TargetReached();
00703   int HasStopped();
00704 
00705   void RandomAnglePos(joint_vec_t& angle);
00706   void RandomCartPos(cart_vec_t& pos);
00707   void RandomCartAbsPos(cart_vec_t& pos);
00708 
00709   // joint_vec_t& GetAngle();
00710 
00715   void GetAngle(joint_vec_t& out) const ;
00716  
00717 
00722   void GetTarget(cart_vec_t& tar) const;
00723 
00728   void GetLocalTarget(cart_vec_t& tar) const {v_copy(target,tar);}
00729 
00730   void GetWeights(cart_vec_t& w_cart, joint_vec_t& w_angle) const;
00731  
00736   virtual void GetTargetAngle(joint_vec_t& out) const ;
00746   virtual void Local2AbsRef(const cart_vec_t& cart_l, cart_vec_t& cart) const;
00747   
00756   virtual void Abs2LocalRef(const cart_vec_t& cart, cart_vec_t& out) const;
00757 
00763   void GetAbsHandPosition(cart_vec_t& out) const;
00764 
00770   void GetLocalHandPosition(cart_vec_t& out) const;
00771 
00772 
00779   float SpuriousAttractorIndex();
00780 
00781 
00782   virtual int GetVisualMode();
00783 
00784 #ifdef LEARNING
00785   //put the function necessary for learning here
00786   float LearningLocal();
00787   float LearningGlobal();
00788 
00789 #endif
00790 
00791 #ifdef WITH_ENVIRONMENT
00792   int SetEnvironment(Environment *env);
00793 #endif
00794 
00795 #ifdef OBSTACLE_AVOIDANCE
00796   int LinkDistanceToObject(int link, EnvironmentObject *obj, float *dist, 
00797                            float *point, cart_vec_t& contact_vector);
00798   int 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);
00799 #endif
00800 
00801 #ifdef DYN_WEIGHTS_OLD
00802   void ResetWeights();
00803 #endif
00804 
00805 #ifdef REST_POS
00806   void SetRestPosWeight(joint_vec_t& w);
00807 #endif
00808 
00809 #ifdef DYN_WEIGHTS
00810 #ifdef PARAM_COS
00811   void SetWeightFunction(float param);
00812 #endif
00813 #endif
00814 
00815  protected:
00816 
00817   void UpdateWeights();
00818   void SetAnglesInRange(joint_vec_t& angle);
00819 };
00820 // external operators
00824 ostream& operator<<(ostream& out, const Reaching& reach);
00828 istream& operator>>(istream& in, Reaching& reach);
00829 
00830 
00831 ostream& operator<<(ostream& out, const ArmConfigList_t manifold);
00832 
00833 typedef Reaching *pReaching;
00834 
00835 
00836 class ReachingRight : public Reaching {
00837         public :
00838         ReachingRight();
00839         ~ReachingRight();
00840 #ifndef LIGHT_VERSION  
00841         void RetrieveJointAngles(pHoap2_JointAngles angles);
00842         int SetActualHoapPosition(pHoap2_JointAngles angles);
00843 #endif
00844 };
00845 
00846 
00850 class ReachingLeft : public Reaching {
00851  protected:
00852   void Real2VirtualAngles(joint_vec_t& angles)const;
00853   void Virtual2RealAngles(joint_vec_t& angles)const;
00854   public :
00855     ReachingLeft();
00856   ~ReachingLeft();
00857 #ifndef LIGHT_VERSION  
00858   void RetrieveJointAngles(pHoap2_JointAngles angles);
00859   int SetActualHoapPosition(pHoap2_JointAngles angles);
00860 #endif
00861   //int SetTarget(cart_vec_t& new_target);
00862   virtual void Local2AbsRef(const cart_vec_t& cart_l, cart_vec_t& cart) const;
00863   virtual void Abs2LocalRef(const cart_vec_t& cart, cart_vec_t& out) const;
00864   virtual void GetTargetAngle(joint_vec_t& angle) const;
00865   //  virtual void Angle2Cart(joint_vec_t& angle, cart_vec_t& pos);
00866 
00867 
00868 #endif //NOT_YET
00869 };
00870 
00871 
00872 
00873 
00874 #endif
00875 
 All Data Structures Functions Variables

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