Fawkes API  Fawkes Development Version
qa_robot.cpp
00001 
00002 /***************************************************************************
00003  *  qa_robot.cpp - QA for OpenRAVE Environment 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/environment.h>
00028 #include <plugins/openrave/robot.h>
00029 #include <plugins/openrave/manipulators/katana6M180.h>
00030 #include <logging/console.h>
00031 #include <cstdio>
00032 #include <iostream>
00033 #include <sstream>
00034 #include <vector>
00035 
00036 using namespace fawkes;
00037 using namespace std;
00038 
00039 void
00040 printVector(vector<float> &v)
00041 {
00042   stringstream s;
00043   //printf("## size:%u \n", v.size());
00044   for(unsigned int i=0; i<v.size(); i++)
00045   {
00046     s << "(" << i << ")" << v[i] << "    ";
00047     //printf("## %u:)%f \n", i, v[i]);
00048   }
00049   printf("%s \n", s.str().c_str());
00050 }
00051 
00052 int
00053 main(int argc, char **argv)
00054 {
00055   printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00056 
00057   string robotFile = SRCDIR"/../manipulators/katana.robot.xml";
00058 
00059   ConsoleLogger* cl = new ConsoleLogger();
00060 
00061   OpenRaveManipulator* manip = new OpenRaveManipulatorKatana6M180(6, 5);
00062   OpenRaveRobot* robot = new OpenRaveRobot(cl);
00063   OpenRaveEnvironment* env = new OpenRaveEnvironment(cl);
00064 
00065   env->create();
00066 
00067   try {
00068     robot->load(robotFile, env);
00069   } catch (Exception &e) {
00070     cl->log_error("qa_robot", "error:%s", e.what());
00071     return 0;
00072   }
00073 
00074   // configure manip
00075   manip->add_motor(0,0);
00076   manip->add_motor(1,1);
00077   manip->add_motor(2,2);
00078   manip->add_motor(4,3);
00079   manip->add_motor(5,4);
00080   robot->set_manipulator(manip);
00081 
00082   env->add_robot(robot);
00083   robot->set_ready();
00084   env->lock();
00085 
00086 
00087   vector<float> val, v;
00088   val.push_back(0.1);
00089   val.push_back(0.2);
00090   val.push_back(0.3);
00091   val.push_back(0.4);
00092   val.push_back(0.5);
00093 
00094   manip->set_angles_device(val);
00095   manip->get_angles(v);
00096   printVector(v);
00097   manip->get_angles_device(v);
00098   printVector(v);
00099 
00100 
00101   env->start_viewer();
00102 
00103   //print angles taken from OpenRAVE Model (can be modified in GUI)
00104   while(1) {
00105     robot->update_manipulator();
00106     manip->get_angles(v);
00107     printVector(v);
00108     manip->get_angles_device(v);
00109     printVector(v);
00110     usleep(1000*500);
00111   }
00112 
00113 
00114   env->destroy();
00115 
00116   return 0;
00117 }
00118 
00119 
00120 /// @endcond