Fawkes API  Fawkes Development Version
controller_openrave.cpp
1 
2 /***************************************************************************
3  * controller_openrave.cpp - OpenRAVE Controller class for katana arm
4  *
5  * Created: Sat Jan 07 16:10:54 2012
6  * Copyright 2012 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.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "controller_openrave.h"
24 #include "exception.h"
25 
26 #include <core/exceptions/software.h>
27 
28 #ifdef HAVE_OPENRAVE
29 #include <plugins/openrave/environment.h>
30 #include <plugins/openrave/robot.h>
31 #include <plugins/openrave/manipulator.h>
32 
33 #include <cmath>
34 
35 using namespace OpenRAVE;
36 #endif
37 
38 namespace fawkes {
39 #if 0 /* just to make Emacs auto-indent happy */
40 }
41 #endif
42 
43 /** @class KatanaControllerOpenrave <plugins/katana/controller_kni.h>
44  * Controller class for a Neuronics Katana, using libkni to interact
45  * with the real Katana arm.
46  * @author Bahram Maleki-Fard
47  */
48 
49 #ifdef HAVE_OPENRAVE
50 
51 /** Constructor.
52  * @param openrave pointer to OpenRaveConnector aspect.
53  */
54 KatanaControllerOpenrave::KatanaControllerOpenrave(fawkes::OpenRaveConnector* openrave)
55 {
56  __openrave = openrave;
57  __OR_env = NULL;
58  __OR_robot = NULL;
59  __OR_manip = NULL;
60  __initialized = false;
61 }
62 
63 /** Destructor. */
64 KatanaControllerOpenrave::~KatanaControllerOpenrave()
65 {
66  // Setting to NULL also deletes instance (RefPtr)
67 
68  __openrave = NULL;
69  __OR_env = NULL;
70  __OR_robot = NULL;
71  __OR_manip = NULL;
72 }
73 
74 
75 void
76 KatanaControllerOpenrave::init()
77 {
78  try {
79  __OR_env = __openrave->get_environment();
80  __OR_robot = __openrave->get_active_robot();
81 
82  if( __OR_robot == NULL) {
83  throw fawkes::Exception("Cannot access OpenRaveRobot in current OpenRaveEnvironment.");
84  }
85  // TODO: get robot string and name, compare to this!
86 // __robot->GetName();
87 
88  __OR_manip = __OR_robot->get_manipulator();
89  __env = __OR_env->get_env_ptr();
90  __robot = __OR_robot->get_robot_ptr();
91  __manip = __robot->GetActiveManipulator();
92  __initialized = true;
93 
94  } catch (OpenRAVE::openrave_exception &e) {
95  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
96  }
97 }
98 
99 void
100 KatanaControllerOpenrave::set_max_velocity(unsigned int vel)
101 {
102  check_init();
103  try {
104  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
105  std::vector<dReal> v;
106  __OR_manip->get_angles(v);
107  v.assign(v.size(), (dReal)(vel / 100.0));
108 
109  __robot->SetActiveDOFVelocities(v);
110  } catch( /*OpenRAVE*/::openrave_exception &e) {
111  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
112  }
113 }
114 
115 
116 bool
117 KatanaControllerOpenrave::final()
118 {
119  check_init();
120  return __robot->GetController()->IsDone();
121 }
122 
123 bool
124 KatanaControllerOpenrave::joint_angles()
125 {
126  return true;
127 }
128 bool
129 KatanaControllerOpenrave::joint_encoders()
130 {
131  return false;
132 }
133 
134 void
135 KatanaControllerOpenrave::calibrate()
136 {
137  // do nothing, arm in OpenRAVE does not need calibration
138 }
139 
140 void
141 KatanaControllerOpenrave::stop()
142 {
143  check_init();
144  try {
145  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
146  std::vector<dReal> v;
147  __robot->GetActiveDOFValues(v);
148  __robot->SetActiveDOFValues(v);
149  } catch( /*OpenRAVE*/::openrave_exception &e) {
150  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
151  }
152 }
153 
154 void
155 KatanaControllerOpenrave::turn_on()
156 {
157 }
158 
159 void
160 KatanaControllerOpenrave::turn_off()
161 {
162 }
163 
164 void
165 KatanaControllerOpenrave::read_coordinates(bool refresh)
166 {
167  check_init();
168  try {
169  update_manipulator();
170  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
171  Transform tf = __manip->GetEndEffectorTransform();
172  __x = tf.trans[0];
173  __y = tf.trans[1];
174  __z = tf.trans[2];
175  //transform quat to euler.
176  TransformMatrix m = matrixFromQuat(tf.rot);
177  std::vector<dReal> v;
178  __OR_manip->get_angles_device(v);
179  __phi = v.at(0) - 0.5*M_PI; //phi is directly derivable from joint0
180  __psi = 0.5*M_PI - v.at(4); //psi is directly derivable from joint4
181  __theta = acos(m.m[10]);
182  //theta has correct range from 0-360°, but need to check if sign is also correct. use sinus for that
183  if( asin(m.m[2] / sin(__phi)) < 0.0 )
184  __theta *= -1.0;
185  } catch( /*OpenRAVE*/::openrave_exception &e ) {
186  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
187  }
188 }
189 
190 void
191 KatanaControllerOpenrave::read_motor_data()
192 {
193  //no need, simulation loop should always be running
194 }
195 
196 void
197 KatanaControllerOpenrave::read_sensor_data()
198 {
199  //no need, simulation loop should always be running
200 }
201 
202 void
203 KatanaControllerOpenrave::gripper_open(bool blocking)
204 {
205 
206 }
207 
208 void
209 KatanaControllerOpenrave::gripper_close(bool blocking)
210 {
211 
212 }
213 
214 void
215 KatanaControllerOpenrave::move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking)
216 {
217  // This method is only here for conveniance, used by KNI
218  throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept Euler Rotation");
219 }
220 
221 void
222 KatanaControllerOpenrave::move_to(std::vector<int> encoders, bool blocking)
223 {
224  throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
225 }
226 
227 void
228 KatanaControllerOpenrave::move_to(std::vector<float> angles, bool blocking)
229 {
230  check_init();
231  try {
232  std::vector<dReal> v;
233  __OR_manip->set_angles_device(angles);
234 
235  __OR_manip->get_angles(v);
236  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
237  __robot->SetActiveDOFValues(v);
238  usleep(2000);
239  } catch( /*OpenRAVE*/::openrave_exception &e) {
240  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
241  }
242 
243  if( blocking ) {
244  wait_finished();
245  }
246 }
247 
248 void
249 KatanaControllerOpenrave::move_motor_to(unsigned short id, int enc, bool blocking)
250 {
251  throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
252 }
253 
254 void
255 KatanaControllerOpenrave::move_motor_to(unsigned short id, float angle, bool blocking)
256 {
257  check_init();
258  try {
259  std::vector<dReal> v;
260  __OR_manip->get_angles_device(v);
261  v.at(id) = (dReal)angle;
262  __OR_manip->set_angles_device(v);
263 
264  __OR_manip->get_angles(v);
265  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
266  __robot->SetActiveDOFValues(v);
267  usleep(2000);
268  } catch( /*OpenRAVE*/::openrave_exception &e) {
269  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
270  }
271 
272  if( blocking ) {
273  wait_finished();
274  }
275 }
276 
277 void
278 KatanaControllerOpenrave::move_motor_by(unsigned short id, int enc, bool blocking)
279 {
280  throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
281 }
282 
283 void
284 KatanaControllerOpenrave::move_motor_by(unsigned short id, float angle, bool blocking)
285 {
286  check_init();
287  try {
288  std::vector<dReal> v;
289  __OR_manip->get_angles_device(v);
290  v.at(id) += (dReal)angle;
291  __OR_manip->set_angles_device(v);
292 
293  __OR_manip->get_angles(v);
294  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
295  __robot->SetActiveDOFValues(v);
296  usleep(2000);
297  } catch( /*OpenRAVE*/::openrave_exception &e) {
298  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
299  }
300 
301  if( blocking ) {
302  wait_finished();
303  }
304 }
305 
306 
307 // getters
308 double
309 KatanaControllerOpenrave::x()
310 {
311  return __x;
312 }
313 
314 double
315 KatanaControllerOpenrave::y()
316 {
317  return __y;
318 }
319 
320 double
321 KatanaControllerOpenrave::z()
322 {
323  return __z;
324 }
325 
326 double
327 KatanaControllerOpenrave::phi()
328 {
329  return __phi;
330 }
331 
332 double
333 KatanaControllerOpenrave::theta()
334 {
335  return __theta;
336 }
337 
338 double
339 KatanaControllerOpenrave::psi()
340 {
341  return __psi;
342 }
343 
344 void
345 KatanaControllerOpenrave::get_sensors(std::vector<int>& to, bool refresh)
346 {
347  check_init();
348  to.clear();
349  to.resize(0);
350 }
351 
352 void
353 KatanaControllerOpenrave::get_encoders(std::vector<int>& to, bool refresh)
354 {
355  throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
356 }
357 
358 void
359 KatanaControllerOpenrave::get_angles(std::vector<float>& to, bool refresh)
360 {
361  check_init();
362  try {
363  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
364  std::vector<dReal> v;
365  __robot->GetActiveDOFValues(v);
366  __OR_manip->set_angles(v);
367 
368  __OR_manip->get_angles_device(to);
369  } catch( /*OpenRAVE*/::openrave_exception &e) {
370  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
371  }
372 }
373 
374 
375 void
376 KatanaControllerOpenrave::update_manipulator()
377 {
378  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
379  __manip = __robot->GetActiveManipulator();
380 }
381 
382 void
383 KatanaControllerOpenrave::wait_finished()
384 {
385  while( !final() ) {
386  usleep(1000);
387  }
388 }
389 
390 bool
391 KatanaControllerOpenrave::motor_oor(unsigned short id)
392 {
393  check_init();
394  std::vector<dReal> v;
395  __OR_manip->get_angles_device(v);
396 
397  return id > v.size();
398 }
399 
400 void
401 KatanaControllerOpenrave::check_init()
402 {
403  if( !__initialized ) {
404  init();
405  // init() will throw an exception if it fails
406  }
407 }
408 
409 #endif // HAVE_OPENRAVE
410 
411 } // end of namespace fawkes
Fawkes library namespace.
Unsupported command.
Definition: exception.h:50
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for a OpenRave connection creator.
virtual OpenRaveEnvironment * get_environment() const =0
Get pointer to OpenRaveEnvironment object.