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