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
37using namespace OpenRAVE;
38#endif
39
40namespace 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 */
53KatanaControllerOpenrave::KatanaControllerOpenrave(fawkes::OpenRaveConnector *openrave)
54{
55 openrave_ = openrave;
56 initialized_ = false;
57}
58
59/** Destructor. */
60KatanaControllerOpenrave::~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
70void
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
94void
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
110bool
112{
113 check_init();
114 return robot_->GetController()->IsDone();
115}
116
117bool
119{
120 return true;
121}
122bool
124{
125 return false;
126}
127
128void
130{
131 // do nothing, arm in OpenRAVE does not need calibration
132}
133
134void
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
148void
150{
151}
152
153void
155{
156}
157
158void
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
184void
186{
187 //no need, simulation loop should always be running
188}
189
190void
192{
193 //no need, simulation loop should always be running
194}
195
196void
198{
199}
200
201void
203{
204}
205
206void
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
219void
220KatanaControllerOpenrave::move_to(std::vector<int> encoders, bool blocking)
221{
222 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
223}
224
225void
226KatanaControllerOpenrave::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
246void
247KatanaControllerOpenrave::move_motor_to(unsigned short id, int enc, bool blocking)
248{
249 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
250}
251
252void
253KatanaControllerOpenrave::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
275void
276KatanaControllerOpenrave::move_motor_by(unsigned short id, int enc, bool blocking)
277{
278 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
279}
280
281void
282KatanaControllerOpenrave::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
305double
307{
308 return x_;
309}
310
311double
313{
314 return y_;
315}
316
317double
319{
320 return z_;
321}
322
323double
325{
326 return phi_;
327}
328
329double
331{
332 return theta_;
333}
334
335double
337{
338 return psi_;
339}
340
341void
342KatanaControllerOpenrave::get_sensors(std::vector<int> &to, bool refresh)
343{
344 check_init();
345 to.clear();
346 to.resize(0);
347}
348
349void
350KatanaControllerOpenrave::get_encoders(std::vector<int> &to, bool refresh)
351{
352 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders");
353}
354
355void
356KatanaControllerOpenrave::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
371void
372KatanaControllerOpenrave::update_manipulator()
373{
374 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
375 manip_ = robot_->GetActiveManipulator();
376}
377
378void
379KatanaControllerOpenrave::wait_finished()
380{
381 while (!final()) {
382 usleep(1000);
383 }
384}
385
386bool
387KatanaControllerOpenrave::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
396void
397KatanaControllerOpenrave::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
virtual double phi()=0
Get x-coordinate of latest endeffector position.
virtual void gripper_open(bool blocking=false)=0
Open Gripper.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void set_max_velocity(unsigned int vel)=0
Set maximum velocity.
virtual double theta()=0
Get theta-rotation of latest endeffector orientation.
virtual void gripper_close(bool blocking=false)=0
Close Gripper.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual double psi()=0
Get psi-rotation of latest endeffector orientation.
virtual double y()=0
Get y-coordinate of latest endeffector position.
virtual void init()=0
Initialize controller.
virtual double z()=0
Get z-coordinate of latest endeffector position.
virtual void move_motor_to(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor to encoder value.
virtual void move_motor_by(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor by encoder value (i.e.
virtual bool joint_encoders()=0
Check if controller provides joint encoder values.
virtual double x()=0
Get x-coordinate of latest endeffector position.
virtual bool final()=0
Check if movement is final.
virtual void turn_on()=0
Turn on arm/motors.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)=0
Get sensor values.
virtual void calibrate()=0
Calibrate the arm.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
virtual void stop()=0
Stop movement immediately.
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)=0
Move endeffctor to given coordinates.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
virtual void read_coordinates(bool refresh=false)=0
Store current coordinates of endeeffctor.
virtual void turn_off()=0
Turn off arm/motors.
Unsupported command.
Definition: exception.h:50
Interface for a OpenRave connection creator.
Fawkes library namespace.