BodySchema.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 __BODY_SCHEMA_H__
00020 #define __BODY_SCHEMA_H__
00021 
00022 #include "assert.h"
00023 #include "Vector.h"
00024 #include "Matrix.h"
00025 #include <iostream>
00026 #include <fstream>
00027 
00028 using namespace std;
00029 
00030 extern int cartesian_dim;
00031 extern int joint_angle_dim;
00032 
00033 class ConfigurationVector: public Vector{
00034 public:
00035     ConfigurationVector():Vector(joint_angle_dim){};
00036     ConfigurationVector& operator=(const Vector& v){if(v.Size()==row)
00037             {Vector::operator=(v);}else{cout<<"bad joint space vector dimension"<<endl;}return *this;}
00038 };
00039 
00040 
00041 class PositionVector: public Vector{
00042 public: 
00043     PositionVector():Vector(cartesian_dim){};
00044     PositionVector& operator=(const Vector& v){if(v.Size()==row)
00045             {Vector::operator=(v);}else{cout<<"bad cartesian space vector dimension"<<endl;}return *this;}
00046 };
00047 
00048 typedef ConfigurationVector joint_vec_t;
00049 
00050 typedef PositionVector cart_vec_t;
00051 
00052 
00057 class BodySchema
00058 {
00059 protected:
00063     joint_vec_t min_angle;
00067     joint_vec_t max_angle;
00068     
00069 
00070     int ik_trials;
00071 public:
00072     
00073     BodySchema()
00074     {min_angle.Zero();min_angle-= PIf/2; max_angle.Zero();max_angle += PIf/2;ik_trials=10; };
00075     void SetIkTrials(int n){ik_trials = n;};
00076     virtual ~BodySchema(){};
00077     virtual int GetNbJoints(){return joint_angle_dim;}
00078     virtual void Angle2Cart(joint_vec_t& angle, cart_vec_t& out){};
00079     virtual void Angle2Cart(cart_vec_t& out){};
00080     virtual void Jacobian(Matrix& jac){};
00084     virtual void FullJacobian(Matrix& jac){};
00085     virtual void Jacobian(joint_vec_t& v, Matrix& jac){};
00086     virtual void Print(ostream& out)const{out<<"no printing function defined"<<endl;};
00087     virtual void Load(istream& in){};
00088     virtual void Load(const char *filename){ifstream fin;fin.open(filename);Load(fin);fin.close();};  
00089     virtual void GetAngles(joint_vec_t& angles){};
00090     virtual void GetRandomAngle(joint_vec_t& angles)
00091     {angles.Random(); min_angle.Add(angles^(max_angle-min_angle),angles);}
00092     virtual void SetRandomAngle(joint_vec_t& angles){};
00093     virtual int InverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){return 0;};
00094     virtual int InverseKinematics(const cart_vec_t& pos,joint_vec_t& start_angle,joint_vec_t& angles){return 0;};
00095     virtual void GetWeights(cart_vec_t& cart_weights, joint_vec_t& joint_weights){};
00096     virtual void SetAnglesInRange(joint_vec_t& angle){
00097         int n=min(angle.Size(),min_angle.Size());//min_angle and max_angle are assumed to be the same size
00098         for(int i=0;i<n;i++)angle[i] = max(min_angle[i],min(max_angle[i],angle[i]));};
00099     
00103     float AngleInClosestRange(int i,float cand){
00104         if(cand > max_angle[i])return cand-max_angle[i]<min_angle[i]+2*PIf-cand? max_angle[i]:min_angle[i];
00105         if(cand < min_angle[i])return min_angle[i]-cand<cand-max_angle[i]+2*PIf? min_angle[i]:max_angle[i];
00106         return cand;
00107     }
00108     
00109     virtual void UpdateDimension(){min_angle.Resize(joint_angle_dim);max_angle.Resize(joint_angle_dim);}
00110     
00111     virtual int AnglesInRange(joint_vec_t& angles)
00112     {for(unsigned int i=0;i<angles.Size();i++){if(angles[i]<min_angle[i] || angles[i]>max_angle[i])return 0;}return 1;}
00113     virtual void SetPosition(joint_vec_t& angles){};
00114     virtual void GetAnglesLowerBound(joint_vec_t& lb){lb =min_angle;};
00115     virtual void GetAnglesUpperBound(joint_vec_t& ub){ub =max_angle;};
00116     virtual void SetAnglesLowerBound(joint_vec_t& lb){min_angle=lb;};
00117     virtual void SetAnglesUpperBound(joint_vec_t& ub){max_angle=ub;};
00118     void SetAngleBounds(joint_vec_t& lb, joint_vec_t& ub)
00119     {SetAnglesUpperBound(ub);SetAnglesLowerBound(lb);} 
00120     virtual joint_vec_t& GetAnglesLowerBound(){return min_angle;};
00121     virtual joint_vec_t& GetAnglesUpperBound(){return max_angle;};
00122     virtual float CartesianDistance(cart_vec_t& p1, cart_vec_t& p2){return (p1-p2).Norm();};
00128     virtual float* Serialize(float *data, int offset){return NULL;};
00134     virtual int Deserialize(const float *data,int offset){return 0;};
00141     virtual int Deserialize(const float *data,int from, int to){return 0;};
00142 };
00143 
00144 
00145 
00146 /* ostream& operator<<(ostream& out, const BodySchema& bs){bs.Print(out);return out;}; */
00147 /* istream& operator>>(istream& in, BodySchema& bs){bs.Load(in);return in;}; */
00148 
00149 
00150 #endif
 All Data Structures Functions Variables

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