Fawkes API
Fawkes Development Version
|
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