Fawkes API Fawkes Development Version
controller_kni.cpp
1
2/***************************************************************************
3 * controller_kni.cpp - KNI Controller class for katana arm
4 *
5 * Created: Tue Jan 03 11:40:31 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_kni.h"
24
25#include "exception.h"
26
27#include <common/MathHelperFunctions.h>
28#include <core/exceptions/software.h>
29
30#include <kniBase.h>
31
32namespace fawkes {
33
34/** @class KatanaControllerKni <plugins/katana/controller_kni.h>
35 * Controller class for a Neuronics Katana, using libkni to interact
36 * with the real Katana arm.
37 * @author Bahram Maleki-Fard
38 */
39
40/** Constructor. */
42: cfg_device_("/dev/ttyS0"), cfg_kni_conffile_("/etc/kni3/hd300/katana6M180.cfg")
43{
44 cfg_read_timeout_ = 100;
45 cfg_write_timeout_ = 0;
46
47 gripper_last_pos_.clear();
48 gripper_last_pos_.resize(2);
49
50 x_ = y_ = z_ = 0.;
51 phi_ = theta_ = psi_ = 0.;
52}
53
54/** Destructor. */
56{
57 // Setting to NULL also deletes instance (RefPtr)
58 katana_ = NULL;
59
60 device_.reset();
61 protocol_.reset();
62}
63
64/** Setup parameters needed to initialize Katana arm with libkni.
65 * @param device device string, e.g. "/dev/ttyS0"
66 * @param kni_conffile path to kni configfile, e.g. "/etc/kni3/hd300/katana6M180.cfg"
67 * @param read_timeout timeout for read operations, in ms
68 * @param write_timeout timeout for write operations, in ms
69 */
70void
71KatanaControllerKni::setup(std::string &device,
72 std::string &kni_conffile,
73 unsigned int read_timeout,
74 unsigned int write_timeout)
75{
76 cfg_device_ = device;
77 cfg_kni_conffile_ = kni_conffile;
78 cfg_read_timeout_ = read_timeout;
79 cfg_write_timeout_ = write_timeout;
80}
81
82void
84{
85 try {
86 TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, (int)cfg_read_timeout_, (int)cfg_write_timeout_};
87 device_.reset(new CCdlCOM(ccd, cfg_device_.c_str()));
88
89 protocol_.reset(new CCplSerialCRC());
90 protocol_->init(device_.get());
91
92 katana_ = RefPtr<CLMBase>(new CLMBase());
93 katana_->create(cfg_kni_conffile_.c_str(), protocol_.get());
94 katbase_ = katana_->GetBase();
95 sensor_ctrl_ = &katbase_->GetSCT()->arr[0];
96
97 katbase_->recvECH();
98 } catch (/*KNI*/ ::Exception &e) {
99 throw fawkes::Exception("KNI Exception:%s", e.what());
100 }
101
102 try {
103 motor_init_.resize(katana_->getNumberOfMotors());
104 for (unsigned int i = 0; i < motor_init_.size(); i++) {
105 motor_init_.at(i) = *(katbase_->GetMOT()->arr[i].GetInitialParameters());
106 }
107 } catch (/*KNI*/ ::Exception &e) {
108 throw fawkes::Exception("KNI Exception:%s", e.what());
109 }
110}
111
112void
114{
115 try {
116 katana_->setRobotVelocityLimit((int)vel);
117 } catch (/*KNI*/ ::Exception &e) {
118 throw fawkes::Exception("KNI Exception:%s", e.what());
119 }
120}
121
122bool
124{
125 if (active_motors_.size() < 1)
126 return true;
127
128 bool final = true;
129 for (unsigned int i = 0; i < active_motors_.size(); i++) {
130 final &= motor_final(active_motors_.at(i));
131 }
132 cleanup_active_motors();
133 return final;
134}
135
136bool
138{
139 return true;
140}
141bool
143{
144 return true;
145}
146
147void
149{
150 try {
151 katana_->calibrate();
152 } catch (/*KNI*/ ::Exception &e) {
153 throw fawkes::Exception("KNI Exception:%s", e.what());
154 }
155}
156
157void
159{
160 try {
161 katana_->freezeRobot();
162 } catch (/*KNI*/ ::Exception &e) {
163 throw fawkes::Exception("KNI Exception:%s", e.what());
164 }
165}
166
167void
169{
170 try {
171 katana_->switchRobotOn();
172 } catch (/*KNI*/ ::Exception &e) {
173 throw fawkes::Exception("KNI Exception:%s", e.what());
174 }
175}
176
177void
179{
180 try {
181 katana_->switchRobotOff();
182 } catch (/*KNI*/ ::Exception &e) {
183 throw fawkes::Exception("KNI Exception:%s", e.what());
184 }
185}
186
187void
189{
190 try {
191 katana_->getCoordinates(x_, y_, z_, phi_, theta_, psi_, refresh);
192 } catch (/*KNI*/ ::Exception &e) {
193 throw fawkes::Exception("KNI Exception:%s", e.what());
194 }
195}
196
197void
199{
200 try {
201 if (active_motors_.size() == (unsigned short)katana_->getNumberOfMotors()) {
202 katbase_->recvMPS(); // get position for all motors
203 katbase_->recvGMS(); // get status flags for all motors
204 } else {
205 const TKatMOT *mot = katbase_->GetMOT();
206 for (unsigned int i = 0; i < active_motors_.size(); i++) {
207 mot->arr[active_motors_.at(i)].recvPVP(); // get position data for motor
208 }
209 }
210 } catch (/*KNI*/ ::Exception &e) {
211 throw fawkes::Exception("KNI Exception:%s", e.what());
212 }
213}
214
215void
217{
218 try {
219 sensor_ctrl_->recvDAT();
220 } catch (/*KNI*/ ::ParameterReadingException &e) {
221 throw fawkes::Exception("KNI ParameterReadingException:%s", e.what());
222 } catch (/*KNI*/ ::Exception &e) {
223 throw fawkes::Exception("KNI Exception:%s", e.what());
224 }
225}
226
227void
229{
230 try {
231 katana_->openGripper(blocking);
232 } catch (/*KNI*/ ::Exception &e) {
233 throw fawkes::Exception("KNI Exception:%s", e.what());
234 }
235
236 active_motors_.clear();
237 active_motors_.resize(1);
238 active_motors_[0] = katbase_->GetMOT()->cnt - 1;
239
240 gripper_last_pos_.clear();
241 gripper_last_pos_[0] = katbase_->GetMOT()->arr[active_motors_[0]].GetPVP()->pos;
242 gripper_last_pos_[1] = 0; //counter
243}
244
245void
247{
248 try {
249 katana_->closeGripper(blocking);
250 } catch (/*KNI*/ ::Exception &e) {
251 throw fawkes::Exception("KNI Exception:%s", e.what());
252 }
253
254 active_motors_.clear();
255 active_motors_.resize(1);
256 active_motors_[0] = katbase_->GetMOT()->cnt - 1;
257
258 gripper_last_pos_.clear();
259 gripper_last_pos_[0] = katbase_->GetMOT()->arr[active_motors_[0]].GetPVP()->pos;
260 gripper_last_pos_[1] = 0; //counter
261}
262
263void
265 float y,
266 float z,
267 float phi,
268 float theta,
269 float psi,
270 bool blocking)
271{
272 cleanup_active_motors();
273
274 try {
275 katana_->moveRobotTo(x_, y_, z_, phi_, theta_, psi_, blocking);
276 } catch (KNI::NoSolutionException &e) {
277 throw fawkes::KatanaNoSolutionException("KNI NoSolutionException:%s", e.what());
278 } catch (/*KNI*/ ::Exception &e) {
279 throw fawkes::Exception("KNI Exception:%s", e.what());
280 }
281
282 for (short i = 0; i < katana_->getNumberOfMotors(); ++i) {
283 add_active_motor(i);
284 }
285}
286
287void
288KatanaControllerKni::move_to(std::vector<int> encoders, bool blocking)
289{
290 cleanup_active_motors();
291
292 try {
293 katana_->moveRobotToEnc(encoders);
294 } catch (/*KNI*/ ::Exception &e) {
295 throw fawkes::Exception("KNI Exception:%s", e.what());
296 }
297
298 for (unsigned short i = 0; i < encoders.size(); ++i) {
299 add_active_motor(i);
300 }
301}
302
303void
304KatanaControllerKni::move_to(std::vector<float> angles, bool blocking)
305{
306 std::vector<int> encoders;
307
308 try {
309 for (unsigned int i = 0; i < angles.size(); i++) {
310 encoders.push_back(KNI_MHF::rad2enc((double)angles.at(i),
311 motor_init_.at(i).angleOffset,
312 motor_init_.at(i).encodersPerCycle,
313 motor_init_.at(i).encoderOffset,
314 motor_init_.at(i).rotationDirection));
315 }
316 } catch (/*KNI*/ ::Exception &e) {
317 throw fawkes::Exception("KNI Exception:%s", e.what());
318 }
319
320 move_to(encoders, blocking);
321}
322
323void
324KatanaControllerKni::move_motor_to(unsigned short id, int enc, bool blocking)
325{
326 if (motor_oor(id))
327 throw fawkes::KatanaOutOfRangeException("Motor out of range.");
328
329 cleanup_active_motors();
330
331 try {
332 katana_->moveMotorToEnc(id, enc);
333 } catch (/*KNI*/ ::Exception &e) {
334 throw fawkes::Exception("KNI Exception:%s", e.what());
335 }
336
337 add_active_motor(id);
338}
339
340void
341KatanaControllerKni::move_motor_to(unsigned short id, float angle, bool blocking)
342{
343 if (motor_oor(id))
344 throw fawkes::KatanaOutOfRangeException("Motor out of range.");
345
346 cleanup_active_motors();
347
348 try {
349 katana_->moveMotorTo(id, angle);
350 } catch (/*KNI*/ ::Exception &e) {
351 throw fawkes::Exception("KNI Exception:%s", e.what());
352 }
353
354 add_active_motor(id);
355}
356
357void
358KatanaControllerKni::move_motor_by(unsigned short id, int enc, bool blocking)
359{
360 if (motor_oor(id))
361 throw fawkes::KatanaOutOfRangeException("Motor out of range.");
362
363 cleanup_active_motors();
364
365 try {
366 katana_->moveMotorByEnc(id, enc);
367 } catch (/*KNI*/ ::Exception &e) {
368 throw fawkes::Exception("KNI Exception:%s", e.what());
369 }
370
371 add_active_motor(id);
372}
373
374void
375KatanaControllerKni::move_motor_by(unsigned short id, float angle, bool blocking)
376{
377 if (motor_oor(id))
378 throw fawkes::KatanaOutOfRangeException("Motor out of range.");
379
380 cleanup_active_motors();
381
382 try {
383 katana_->moveMotorBy(id, angle);
384 } catch (/*KNI*/ ::Exception &e) {
385 throw fawkes::Exception("KNI Exception:%s", e.what());
386 }
387
388 add_active_motor(id);
389}
390
391// getters
392double
394{
395 return x_;
396}
397
398double
400{
401 return y_;
402}
403
404double
406{
407 return z_;
408}
409
410double
412{
413 return phi_;
414}
415
416double
418{
419 return theta_;
420}
421
422double
424{
425 return psi_;
426}
427
428void
429KatanaControllerKni::get_sensors(std::vector<int> &to, bool refresh)
430{
431 if (refresh)
433
434 try {
435 const TSctDAT *sensor_data = sensor_ctrl_->GetDAT();
436
437 const int num_sensors = (size_t)sensor_data->cnt;
438 to.clear();
439 to.resize(num_sensors);
440
441 for (int i = 0; i < num_sensors; ++i) {
442 to[i] = sensor_data->arr[i];
443 }
444 } catch (/*KNI*/ ::Exception &e) {
445 throw fawkes::Exception("KNI Exception:%s", e.what());
446 }
447}
448
449void
450KatanaControllerKni::get_encoders(std::vector<int> &to, bool refresh)
451{
452 try {
453 std::vector<int> encoders = katana_->getRobotEncoders(refresh);
454
455 to.clear();
456 to = encoders;
457 } catch (/*KNI*/ ::Exception &e) {
458 throw fawkes::Exception("KNI Exception:%s", e.what());
459 }
460}
461
462void
463KatanaControllerKni::get_angles(std::vector<float> &to, bool refresh)
464{
465 try {
466 std::vector<int> encoders = katana_->getRobotEncoders(refresh);
467
468 to.clear();
469 for (unsigned int i = 0; i < encoders.size(); i++) {
470 to.push_back(KNI_MHF::enc2rad(encoders.at(i),
471 motor_init_.at(i).angleOffset,
472 motor_init_.at(i).encodersPerCycle,
473 motor_init_.at(i).encoderOffset,
474 motor_init_.at(i).rotationDirection));
475 }
476 } catch (/*KNI*/ ::Exception &e) {
477 throw fawkes::Exception("KNI Exception:%s", e.what());
478 }
479}
480
481bool
482KatanaControllerKni::motor_oor(unsigned short id)
483{
484 return id > (unsigned short)katana_->getNumberOfMotors();
485}
486
487bool
488KatanaControllerKni::motor_final(unsigned short id)
489{
490 CMotBase mot = katbase_->GetMOT()->arr[id];
491 if (mot.GetPVP()->msf == MSF_MOTCRASHED)
492 throw fawkes::KatanaMotorCrashedException("Motor %u crashed.", id);
493
494 // extra check for gripper, consider final if not moved for a while
495 unsigned short gripper_not_moved = 0;
496 if (id == katbase_->GetMOT()->cnt - 1) {
497 if (gripper_last_pos_[0] == mot.GetPVP()->pos) {
498 gripper_last_pos_[1] += 1;
499 } else {
500 gripper_last_pos_[0] = mot.GetPVP()->pos;
501 gripper_last_pos_[1] = 0;
502 }
503 gripper_not_moved = gripper_last_pos_[1];
504 }
505
506 return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10) or (gripper_not_moved > 3);
507}
508
509void
510KatanaControllerKni::cleanup_active_motors()
511{
512 for (unsigned int i = 0; i < active_motors_.size(); ++i) {
513 if (motor_final(active_motors_.at(i))) {
514 active_motors_.erase(active_motors_.begin() + i);
515 --i;
516 }
517 }
518}
519
520void
521KatanaControllerKni::add_active_motor(unsigned short id)
522{
523 for (unsigned int i = 0; i < active_motors_.size(); ++i) {
524 if (active_motors_.at(i) == id) {
525 return;
526 }
527 }
528 active_motors_.push_back(id);
529}
530
531} // end of namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void get_angles(std::vector< float > &to, bool refresh=false)
Get angle values of joints/motors.
virtual void calibrate()
Calibrate the arm.
virtual bool joint_angles()
Check if controller provides joint angle values.
virtual void gripper_close(bool blocking=false)
Close Gripper.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)
Get encoder values of joints/motors.
virtual bool joint_encoders()
Check if controller provides joint encoder values.
virtual double y()
Get y-coordinate of latest endeffector position.
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)
Move endeffctor to given coordinates.
virtual void turn_off()
Turn off arm/motors.
virtual double psi()
Get psi-rotation of latest endeffector orientation.
virtual void gripper_open(bool blocking=false)
Open Gripper.
virtual double theta()
Get theta-rotation of latest endeffector orientation.
virtual void stop()
Stop movement immediately.
virtual double z()
Get z-coordinate of latest endeffector position.
virtual bool final()
Check if movement is final.
virtual void read_sensor_data()
Read all sensor data from device into controller libray.
virtual void setup(std::string &device, std::string &kni_conffile, unsigned int read_timeout, unsigned int write_timeout)
Setup parameters needed to initialize Katana arm with libkni.
virtual void init()
Initialize controller.
virtual void move_motor_by(unsigned short id, int enc, bool blocking=false)
Move single joint/motor by encoder value (i.e.
virtual double phi()
Get x-coordinate of latest endeffector position.
virtual ~KatanaControllerKni()
Destructor.
virtual void set_max_velocity(unsigned int vel)
Set maximum velocity.
virtual void turn_on()
Turn on arm/motors.
virtual void read_motor_data()
Read motor data of currently active joints from device into controller libray.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)
Get sensor values.
virtual void read_coordinates(bool refresh=false)
Store current coordinates of endeeffctor.
virtual void move_motor_to(unsigned short id, int enc, bool blocking=false)
Move single joint/motor to encoder value.
virtual double x()
Get x-coordinate of latest endeffector position.
At least one motor crashed.
Definition: exception.h:44
No joint configuration for desired target found.
Definition: exception.h:32
At least one motor is out of range.
Definition: exception.h:38
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:447
Fawkes library namespace.