Fawkes API  Fawkes Development Version
qa_manip.cpp
1 
2 /***************************************************************************
3  * qa_manip.cpp - QA for OpenRAVE Manipulator 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/manipulators/katana6M180.h>
28 
29 #include <cstdio>
30 #include <iostream>
31 #include <vector>
32 
33 using namespace fawkes;
34 using namespace std;
35 
36 void
37 printVector(vector<float> v)
38 {
39  printf("## size:%zu \n", v.size());
40  for (unsigned int i = 0; i < v.size(); i++) {
41  printf("## %u:)%f \n", i, v[i]);
42  }
43 }
44 
45 int
46 main(int argc, char **argv)
47 {
48  printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
49 
50  OpenRaveManipulator m(6, 5);
52  vector<float> v;
53  vector<float> val;
54 
55  m.add_motor(0, 0);
56  m.add_motor(1, 1);
57  m.add_motor(2, 2);
58  m.add_motor(4, 3);
59  m.add_motor(5, 4);
60 
61  k.add_motor(0, 0);
62  k.add_motor(1, 1);
63  k.add_motor(2, 2);
64  k.add_motor(4, 3);
65  k.add_motor(5, 4);
66 
67  val.push_back(0.1);
68  val.push_back(0.2);
69  val.push_back(0.3);
70  val.push_back(0.4);
71  val.push_back(0.5);
72 
73  //set angles
74  m.set_angles_device(val);
75  k.set_angles_device(val);
76 
77  //print angles
78  m.get_angles(v);
79  printVector(v);
80 
81  m.get_angles_device(v);
82  printVector(v);
83 
84  k.get_angles(v);
85  printVector(v);
86 
87  k.get_angles_device(v);
88  printVector(v);
89 
90  // test manipulator pointer. same as above, set angles, then print
92 
93  p = new OpenRaveManipulatorKatana6M180(6, 5);
94  p->add_motor(0, 0);
95  p->add_motor(1, 1);
96  p->add_motor(2, 2);
97  p->add_motor(4, 3);
98  p->add_motor(5, 4);
99 
100  p->set_angles_device(val);
101 
102  p->get_angles(v);
103  printVector(v);
104 
105  p->get_angles_device(v);
106  printVector(v);
107 
108  return 0;
109 }
110 
111 /// @endcond
fawkes::OpenRaveManipulatorKatana6M180
Definition: katana6M180.h:36
fawkes::RefPtr< OpenRaveManipulator >
fawkes
fawkes::OpenRaveManipulator::get_angles_device
void get_angles_device(std::vector< T > &to) const
Get motor angles of real device.
Definition: manipulator.h:102
fawkes::OpenRaveManipulator::get_angles
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:89
fawkes::OpenRaveManipulator
Definition: manipulator.h:38
fawkes::OpenRaveManipulator::add_motor
void add_motor(unsigned int number, unsigned int number_device)
Adds a motor to the list(vector) of motors.
Definition: manipulator.cpp:59
fawkes::OpenRaveManipulator::set_angles_device
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:147