Fawkes API Fawkes Development Version
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 <logging/console.h>
28#include <plugins/openrave/environment.h>
29#include <plugins/openrave/manipulators/katana6M180.h>
30#include <plugins/openrave/robot.h>
31#include <plugins/openrave/types.h>
32
33#include <cstdio>
34#include <iostream>
35#include <sstream>
36#include <vector>
37
38using namespace fawkes;
39using namespace std;
40
41void
42printVector(vector<float> &v)
43{
44 stringstream s;
45 //printf("## size:%u \n", v.size());
46 for (unsigned int i = 0; i < v.size(); i++) {
47 s << "(" << i << ")" << v[i] << " ";
48 //printf("## %u:)%f \n", i, v[i]);
49 }
50 printf("%s \n", s.str().c_str());
51}
52
53int
54main(int argc, char **argv)
55{
56 printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
57
58 string robotFile = SRCDIR "/../manipulators/katana.robot.xml";
59
60 ConsoleLogger *cl = new ConsoleLogger();
61
63 OpenRaveRobotPtr robot(new OpenRaveRobot(cl));
65
66 env->create();
67
68 try {
69 robot->load(robotFile, env);
70 } catch (Exception &e) {
71 cl->log_error("qa_robot", "error:%s", e.what());
72 return 0;
73 }
74
75 // configure manip
76 manip->add_motor(0, 0);
77 manip->add_motor(1, 1);
78 manip->add_motor(2, 2);
79 manip->add_motor(4, 3);
80 manip->add_motor(5, 4);
81 robot->set_manipulator(manip);
82
83 env->add_robot(robot);
84 robot->set_ready();
85 env->lock();
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 env->start_viewer();
101
102 //print angles taken from OpenRAVE Model (can be modified in GUI)
103 while (1) {
104 robot->update_manipulator();
105 manip->get_angles(v);
106 printVector(v);
107 manip->get_angles_device(v);
108 printVector(v);
109 usleep(1000 * 500);
110 }
111
112 env->destroy();
113
114 return 0;
115}
116
117/// @endcond
Interface for logging to stderr.
Definition: console.h:37
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: console.cpp:181
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
OpenRaveEnvironment class.
Definition: environment.h:44
Class containing information about all katana6M180 motors.
Definition: katana6M180.h:31
OpenRAVE Robot class.
Definition: robot.h:38
Fawkes library namespace.