Fawkes API  Fawkes Development Version
qa_manip.cpp
00001 
00002 /***************************************************************************
00003  *  qa_manip.cpp - QA for OpenRAVE Manipulator class
00004  *
00005  *  Created: Thu Sep 16 14:50:34 2010
00006  *  Copyright  2010  Bahram Maleki-Fard, AllemaniACs RoboCup Team
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 // Do not include in api reference
00025 ///@cond QA
00026 
00027 #include <plugins/openrave/manipulators/katana6M180.h>
00028 #include <cstdio>
00029 #include <iostream>
00030 #include <vector>
00031 
00032 using namespace fawkes;
00033 using namespace std;
00034 
00035 void
00036 printVector(vector<float> v)
00037 {
00038   printf("## size:%lu \n", v.size());
00039   for(unsigned int i=0; i<v.size(); i++)
00040   {
00041     printf("## %u:)%f \n", i, v[i]);
00042   }
00043 }
00044 
00045 int
00046 main(int argc, char **argv)
00047 {
00048   printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00049 
00050   OpenRaveManipulator m(6, 5);
00051   OpenRaveManipulatorKatana6M180 k(6, 5);
00052   vector<float> v;
00053   vector<float> val;
00054 
00055   m.add_motor(0,0);
00056   m.add_motor(1,1);
00057   m.add_motor(2,2);
00058   m.add_motor(4,3);
00059   m.add_motor(5,4);
00060 
00061   k.add_motor(0,0);
00062   k.add_motor(1,1);
00063   k.add_motor(2,2);
00064   k.add_motor(4,3);
00065   k.add_motor(5,4);
00066 
00067   val.push_back(0.1);
00068   val.push_back(0.2);
00069   val.push_back(0.3);
00070   val.push_back(0.4);
00071   val.push_back(0.5);
00072 
00073   //set angles
00074   m.set_angles_device(val);
00075   k.set_angles_device(val);
00076 
00077   //print angles
00078   m.get_angles(v);
00079   printVector(v);
00080 
00081   m.get_angles_device(v);
00082   printVector(v);
00083 
00084   k.get_angles(v);
00085   printVector(v);
00086 
00087   k.get_angles_device(v);
00088   printVector(v);
00089 
00090 
00091   // test manipulator pointer. same as above, set angles, then print
00092   OpenRaveManipulator* p;
00093 
00094   p = new OpenRaveManipulatorKatana6M180(6, 5);
00095   p->add_motor(0,0);
00096   p->add_motor(1,1);
00097   p->add_motor(2,2);
00098   p->add_motor(4,3);
00099   p->add_motor(5,4);
00100 
00101   p->set_angles_device(val);
00102 
00103   p->get_angles(v);
00104   printVector(v);
00105 
00106   p->get_angles_device(v);
00107   printVector(v);
00108 
00109 
00110   return 0;
00111 }
00112 
00113 
00114 /// @endcond