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