Fawkes API
Fawkes Development Version
Main Page
Related Pages
Modules
Namespaces
Classes
Files
File List
All
Classes
Namespaces
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Groups
Pages
qa_robot.cpp
1
2
/***************************************************************************
3
* qa_robot.cpp - QA for OpenRAVE Environment class
4
*
5
* Created: Thu Sep 16 14:50:34 2010
6
* Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
7
*
8
****************************************************************************/
9
10
/* This program is free software; you can redistribute it and/or modify
11
* it under the terms of the GNU General Public License as published by
12
* the Free Software Foundation; either version 2 of the License, or
13
* (at your option) any later version. A runtime exception applies to
14
* this software (see LICENSE.GPL_WRE file mentioned below for details).
15
*
16
* This program is distributed in the hope that it will be useful,
17
* but WITHOUT ANY WARRANTY; without even the implied warranty of
18
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19
* GNU Library General Public License for more details.
20
*
21
* Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22
*/
23
24
// Do not include in api reference
25
///@cond QA
26
27
#include <plugins/openrave/environment.h>
28
#include <plugins/openrave/robot.h>
29
#include <plugins/openrave/manipulators/katana6M180.h>
30
#include <logging/console.h>
31
#include <cstdio>
32
#include <iostream>
33
#include <sstream>
34
#include <vector>
35
36
using namespace
fawkes;
37
using namespace
std;
38
39
void
40
printVector(vector<float> &v)
41
{
42
stringstream s;
43
//printf("## size:%u \n", v.size());
44
for
(
unsigned
int
i=0; i<v.size(); i++)
45
{
46
s <<
"("
<< i <<
")"
<< v[i] <<
" "
;
47
//printf("## %u:)%f \n", i, v[i]);
48
}
49
printf(
"%s \n"
, s.str().c_str());
50
}
51
52
int
53
main(
int
argc,
char
**argv)
54
{
55
printf(
"++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"
);
56
57
string
robotFile = SRCDIR
"/../manipulators/katana.robot.xml"
;
58
59
ConsoleLogger
* cl =
new
ConsoleLogger
();
60
61
OpenRaveManipulator
* manip =
new
OpenRaveManipulatorKatana6M180
(6, 5);
62
OpenRaveRobot
* robot =
new
OpenRaveRobot
(cl);
63
OpenRaveEnvironment
* env =
new
OpenRaveEnvironment
(cl);
64
65
env->
create
();
66
67
try
{
68
robot->
load
(robotFile, env);
69
}
catch
(
Exception
&e) {
70
cl->
log_error
(
"qa_robot"
,
"error:%s"
, e.
what
());
71
return
0;
72
}
73
74
// configure manip
75
manip->
add_motor
(0,0);
76
manip->
add_motor
(1,1);
77
manip->
add_motor
(2,2);
78
manip->
add_motor
(4,3);
79
manip->
add_motor
(5,4);
80
robot->
set_manipulator
(manip);
81
82
env->
add_robot
(robot);
83
robot->
set_ready
();
84
env->
lock
();
85
86
87
vector<float> val, v;
88
val.push_back(0.1);
89
val.push_back(0.2);
90
val.push_back(0.3);
91
val.push_back(0.4);
92
val.push_back(0.5);
93
94
manip->
set_angles_device
(val);
95
manip->
get_angles
(v);
96
printVector(v);
97
manip->
get_angles_device
(v);
98
printVector(v);
99
100
101
env->
start_viewer
();
102
103
//print angles taken from OpenRAVE Model (can be modified in GUI)
104
while
(1) {
105
robot->
update_manipulator
();
106
manip->
get_angles
(v);
107
printVector(v);
108
manip->
get_angles_device
(v);
109
printVector(v);
110
usleep(1000*500);
111
}
112
113
114
env->
destroy
();
115
116
return
0;
117
}
118
119
120
/// @endcond
src
plugins
openrave
qa
qa_robot.cpp
Generated by
1.8.1.1