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 <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 
62  OpenRaveRobot* robot = new OpenRaveRobot(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
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:122
virtual void update_manipulator()
Update motor values from OpenRAVE model.
Definition: robot.cpp:212
virtual void start_viewer()
Starts the qt viewer in a separate thread.
virtual void lock()
Lock the environment to prevent changes.
Class containing information about all katana6M180 motors.
Definition: katana6M180.h:33
Interface for logging to stderr.
Definition: console.h:35
Fawkes library namespace.
STL namespace.
virtual void create()
Create and lock the environment.
Definition: environment.cpp:89
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: console.cpp:165
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:67
OpenRAVE Robot class.
Definition: robot.h:41
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Base class for exceptions in Fawkes.
Definition: exception.h:36
OpenRaveEnvironment class.
Definition: environment.h:45
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironment *env)
Load robot from xml file.
Definition: robot.cpp:102
Class containing information about all manipulator motors.
Definition: manipulator.h:35
void get_angles_device(std::vector< T > &to) const
Get motor angles of real device.
Definition: manipulator.h:80
virtual void add_robot(const std::string &filename)
Add a robot into the scene.
virtual void destroy()
Destroy the environment.
virtual void set_ready()
Set robot ready for usage.
Definition: robot.cpp:118
virtual void set_manipulator(fawkes::OpenRaveManipulator *manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
Definition: robot.cpp:201
virtual void add_motor(unsigned int number, unsigned int number_device)
Adds a motor to the list(vector) of motors.
Definition: manipulator.cpp:58