wrapper_control_demo.cpp
Go to the documentation of this file.
1 /**********************************************************************************
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005-2008 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  * You should have received a copy of the GNU General Public License
14  * along with this program; if not, write to the Free Software
15  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
16  **********************************************************************************/
18 // wrapper_control.cpp
19 // demo program for the kni C wrapper dll
20 // PKE/JHA/TPE 2008
22 #include "kniBase.h"
23 #include "kni_wrapper/kni_wrapper.h"
24 #include "kniBase.h"
25 #include <iostream>
26 #include <cstdio>
27 #include <memory>
28 #include <vector>
30 //defines
31 #ifdef WIN32
32 # include <conio.h>
33 #else //LINUX
34 # include "keyboard.h"
35 #endif
36 //globals:
38 const int DEFAULT_ACCELERATION = 2;
39 const int DEFAULT_VELOCITY = 50;
40 int acceleration = DEFAULT_ACCELERATION;
41 int velocity = DEFAULT_VELOCITY;
42 std::vector<TMovement*> movement_vector;
43 bool isOff = false;
45 //helper functions
46 TMovement* allocateMovement() {
47  TMovement *movement = (struct TMovement*)malloc(sizeof(TMovement));
48  TPos *pos = (struct TPos*)malloc(sizeof(TPos));
49  movement->pos = *pos;
50  movement_vector.push_back(movement);
51  return movement;
52 }
53 void freeAllMovements() {
54  for (int i = 0; i < (int)movement_vector.size(); ++i) {
55  free(movement_vector.at(i));
56  }
57 }
58 void displayTrajectoryHelp() {
59  std::cout << "--- Trajectory Menu ------------------------------" << std::endl;
60  std::cout << "?: Display this help o: Switch on/off " << std::endl;
61  std::cout << "v: Change velocity a: Change acceleration " << std::endl;
62  std::cout << "p: Add PTP movement l: Add LINEAR movement " << std::endl;
63  std::cout << "u: Unblock Esc: Exit to main menu " << std::endl;
64  std::cout << "--------------------------------------------------" << std::endl;
65  std::cout << std::endl;
66 }
67 void displayMainHelp() {
68  std::cout << "--- Main Menu ------------------------------------" << std::endl;
69  std::cout << "?: Display this help o: Switch on/off " << std::endl;
70  std::cout << "t: Add trajectory r: Run trajectory " << std::endl;
71  std::cout << "u: Unblock f: Flush movebuffers " << std::endl;
72  std::cout << "Esc: Exit program " << std::endl;
73  std::cout << "--------------------------------------------------" << std::endl;
74  std::cout << std::endl;
75 }
76 void addTrajectory() {
77  std::cout << "Name of Trajectory to create / add? ";
78  std::string name_str;
79  std::cin >> name_str;
80  char * name = new char(strlen(name_str.c_str()));
81  strcpy(name,name_str.c_str());
82  std::cout << std::endl;
83  std::cout << "current velocity: " << velocity << ", current acceleration: " << acceleration << std::endl;
84  std::cout << std::endl;
85  displayTrajectoryHelp();
86  bool loop = true;
87  int input, new_value;
88  TMovement *current_move;
89  while (loop) {
90  input = _getch();
91  switch (input) {
92  case 3:
93  case 4:
94  case 27: //VK_ESCAPE
95  loop = false;
96  continue;
97  case '?':
98  displayTrajectoryHelp();
99  break;
100  case 'o': //VK_O (motors off/on)
101  if(isOff) {
102  allMotorsOn();
103  isOff = false;
104  std::cout << "Motors on" << std::endl << std::endl;
105  } else {
106  allMotorsOff();
107  isOff = true;
108  std::cout << "Motors off" << std::endl << std::endl;
109  }
110  break;
111  case 'v': //VK_V (change velocity)
112  std::cout << "New velocity: ";
113  std::cin >> new_value;
114  if ((new_value >= 10) && (new_value <= 200)) {
115  velocity = new_value;
116  std::cout << " ... OK" << std::endl << std::endl;
117  } else {
118  std::cout << " ... FAILED, has to be min. 10 and max. 200" << std::endl << std::endl;
119  }
120  break;
121  case 'a': //VK_A (change acceleration)
122  std::cout << "New acceleration: ";
123  std::cin >> new_value;
124  if ((new_value >= 1) && (new_value <= 2)) {
125  acceleration = new_value;
126  std::cout << " ... OK" << std::endl << std::endl;
127  } else {
128  std::cout << " ... FAILED, has to be min. 1 and max. 2" << std::endl << std::endl;
129  }
130  break;
131  case 'p': //VK_P (add PTP movement to here)
132  current_move = allocateMovement();
133  getPosition(&(current_move->pos));
134  current_move->transition = PTP;
135  current_move->velocity = velocity;
136  current_move->acceleration = acceleration;
137  pushMovementToStack(current_move, name);
138  std::cout << "Added PTP movement with v=" << velocity << " and a=" << acceleration << " to point:" << std::endl;
139  std::cout.precision(3);
140  std::cout.setf(std::ios::fixed,std::ios::floatfield);
141  std::cout << " X=" << current_move->pos.X;
142  std::cout << ", Y=" << current_move->pos.Y;
143  std::cout << ", Z=" << current_move->pos.Z;
144  std::cout << ", phi=" << current_move->pos.phi;
145  std::cout << ", theta=" << current_move->pos.theta;
146  std::cout << ", psi=" << current_move->pos.psi;
147  std::cout << std::endl << std::endl;
148  break;
149  case 'l': //VK_L (add LINEAR movement to here)
150  current_move = allocateMovement();
151  getPosition(&(current_move->pos));
152  current_move->transition = LINEAR;
153  current_move->velocity = velocity;
154  current_move->acceleration = acceleration;
155  pushMovementToStack(current_move, name);
156  std::cout << "Added LINEAR movement with v=" << velocity << " and a=" << acceleration << " to point:" << std::endl;
157  std::cout.precision(3);
158  std::cout.setf(std::ios::fixed,std::ios::floatfield);
159  std::cout << " X=" << current_move->pos.X;
160  std::cout << ", Y=" << current_move->pos.Y;
161  std::cout << ", Z=" << current_move->pos.Z;
162  std::cout << ", phi=" << current_move->pos.phi;
163  std::cout << ", theta=" << current_move->pos.theta;
164  std::cout << ", psi=" << current_move->pos.psi;
165  std::cout << std::endl << std::endl;
166  break;
167  case 'u': //VK_U (unblock)
168  unblock();
169  std::cout << "Unblocked" << std::endl << std::endl;
170  break;
171  default: //Error message
172  std::cout << "'" << input << "' is not a valid command." << std::endl << std::endl;
173  break;
174  }
175  }
176 }
178 int main(int argc, char *argv[]) {
179  if (argc != 3) {
180  std::cout << "usage: wrapper_control CONFIGFILE IP_ADDR" << std::endl;
181  return 0;
182  }
183  try {
184  initKatana(argv[1], argv[2]);
185  calibrate(0);
186 
187  } catch(Exception &e) {
188  std::cout << "ERROR: " << e.message() << std::endl;
189  return -1;
190  }
191  std::cout << "-------------------------------------------" << std::endl;
192  std::cout << "success: katana initiated" << std::endl;
193  std::cout << "-------------------------------------------" << std::endl;
195  try{
196  displayMainHelp();
197  bool loop = true;
198  int input, repetitions;
199  std::string name_str;
200  char * name;
201  while (loop) {
202  input = _getch();
203  switch (input) {
204  case 3:
205  case 4:
206  case 27: //VK_ESCAPE
207  loop = false;
208  continue;
209  case '?':
210  displayMainHelp();
211  break;
212  case 'o': //VK_O (motors off/on)
213  if(isOff) {
214  allMotorsOn();
215  isOff = false;
216  std::cout << "Motors on" << std::endl << std::endl;
217  } else {
218  allMotorsOff();
219  isOff = true;
220  std::cout << "Motors off" << std::endl << std::endl;
221  }
222  break;
223  case 't': //VK_O (DK to screen)
224  addTrajectory();
225  displayMainHelp();
226  break;
227  case 'r':
228  std::cout << "Name of Trajectory to run? ";
229  std::cin >> name_str;
230  name = new char(strlen(name_str.c_str()));
231  strcpy(name,name_str.c_str());
232  std::cout << std::endl;
233  std::cout << "Number of repetitions? ";
234  std::cin >> repetitions;
235  std::cout << std::endl;
236  runThroughMovementStack(name, repetitions);
237  displayMainHelp();
238  break;
239  case 'u': //VK_U (unblock)
240  unblock();
241  std::cout << "Unblocked" << std::endl << std::endl;
242  break;
243  case 'f': //VK_F (flush)
244  flushMoveBuffers();
245  std::cout << "MoveBuffers flushed" << std::endl << std::endl;
246  break;
247  default: //Error message
248  std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl << std::endl;
249  break;
250  }
251 
252  }
253 
254  // simple movement (was first try)
255  /*TMovement *current_move = allocateMovement();
256  getPosition(&(current_move->pos));
257  current_move->transition = PTP;
258  current_move->velocity = velocity;
259  current_move->acceleration = acceleration;
260  current_move->pos.Z += 50;
261  executeMovement(current_move);*/
262  }
263  catch(Exception &e) {
264  std::cout << "ERROR: " << e.message() << std::endl;
265  freeAllMovements();
266  return -1;
267  }
268  freeAllMovements();
269  return 0;
270 }
272 
273 
274 
TPos::phi
double phi
the orientation of the TCP
Definition: kni_wrapper.h:53
acceleration
int acceleration
Definition: wrapper_control_demo.cpp:40
KNI.getPosition
getPosition
Definition: KNI.py:152
KNI.allMotorsOn
allMotorsOn
Definition: KNI.py:142
displayMainHelp
void displayMainHelp()
Definition: wrapper_control_demo.cpp:67
PTP
Point-to-point movement.
Definition: kni_wrapper.h:58
Exception::message
std::string message() const
Definition: include/common/exception.h:93
DEFAULT_VELOCITY
const int DEFAULT_VELOCITY
Definition: wrapper_control_demo.cpp:39
TMovement::transition
ETransition transition
the transition to this position, PTP or LINEAR
Definition: kni_wrapper.h:67
KNI.pushMovementToStack
pushMovementToStack
Definition: KNI.py:166
KNI.initKatana
initKatana
Definition: KNI.py:154
KNI.unblock
unblock
Definition: KNI.py:178
TMovement::pos
TPos pos
The position, see above struct TPos.
Definition: kni_wrapper.h:65
TPos
extern C because we want to access these interfaces from anywhere:
Definition: kni_wrapper.h:49
TPos::Z
double Z
Definition: kni_wrapper.h:51
kni_wrapper.h
TMovement::velocity
int velocity
The velocitiy for this particular movement.
Definition: kni_wrapper.h:69
KNI.calibrate
calibrate
Definition: KNI.py:143
KNI.runThroughMovementStack
runThroughMovementStack
Definition: KNI.py:167
displayTrajectoryHelp
void displayTrajectoryHelp()
Definition: wrapper_control_demo.cpp:58
movement_vector
std::vector< TMovement * > movement_vector
Definition: wrapper_control_demo.cpp:42
allocateMovement
TMovement * allocateMovement()
Definition: wrapper_control_demo.cpp:46
LINEAR
linear movement
Definition: kni_wrapper.h:60
_getch
int _getch()
Definition: demo/common/keyboard.cpp:58
TMovement::acceleration
int acceleration
the acceleration for this particular movement
Definition: kni_wrapper.h:71
TPos::X
double X
The position in cartesian space.
Definition: kni_wrapper.h:51
TPos::psi
double psi
Definition: kni_wrapper.h:53
TMovement
structure for the
Definition: kni_wrapper.h:63
velocity
int velocity
Definition: wrapper_control_demo.cpp:41
KNI.allMotorsOff
allMotorsOff
Definition: KNI.py:141
TPos::Y
double Y
Definition: kni_wrapper.h:51
flushMoveBuffers
DLLEXPORT int flushMoveBuffers()
flush all the movebuffers
Definition: kni_wrapper.cpp:281
main
int main(int argc, char *argv[])
Definition: wrapper_control_demo.cpp:178
isOff
bool isOff
Definition: wrapper_control_demo.cpp:43
addTrajectory
void addTrajectory()
Definition: wrapper_control_demo.cpp:76
kni_demo.repetitions
repetitions
Definition: kni_demo.py:76
DEFAULT_ACCELERATION
const int DEFAULT_ACCELERATION
Definition: wrapper_control_demo.cpp:38
freeAllMovements
void freeAllMovements()
Definition: wrapper_control_demo.cpp:53
TPos::theta
double theta
Definition: kni_wrapper.h:53

kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:17