23 #include "controller_kni.h"
24 #include "exception.h"
26 #include <core/exceptions/software.h>
29 #include <common/MathHelperFunctions.h>
43 KatanaControllerKni::KatanaControllerKni()
46 __cfg_device =
"/dev/ttyS0";
47 __cfg_kni_conffile =
"/etc/kni3/hd300/katana6M180.cfg";
48 __cfg_read_timeout = 100;
49 __cfg_write_timeout = 0;
51 __gripper_last_pos.clear();
52 __gripper_last_pos.resize(2);
56 KatanaControllerKni::~KatanaControllerKni()
72 KatanaControllerKni::setup(std::string& device, std::string& kni_conffile,
73 unsigned int read_timeout,
unsigned int write_timeout)
75 __cfg_device = device;
76 __cfg_kni_conffile = kni_conffile;
77 __cfg_read_timeout = read_timeout;
78 __cfg_write_timeout = write_timeout;
82 KatanaControllerKni::init()
85 TCdlCOMDesc ccd = {0, 57600, 8,
'N', 1, (int)__cfg_read_timeout, (
int)__cfg_write_timeout};
86 __device.reset(
new CCdlCOM(ccd, __cfg_device.c_str()));
88 __protocol.reset(
new CCplSerialCRC());
89 __protocol->init(__device.get());
92 __katana->create(__cfg_kni_conffile.c_str(), __protocol.get());
93 __katbase = __katana->GetBase();
94 __sensor_ctrl = &__katbase->GetSCT()->arr[0];
102 __motor_init.resize(__katana->getNumberOfMotors());
103 for(
unsigned int i=0; i<__motor_init.size(); i++) {
104 __motor_init.at(i) = *(__katbase->GetMOT()->arr[i].GetInitialParameters());
112 KatanaControllerKni::set_max_velocity(
unsigned int vel)
115 __katana->setRobotVelocityLimit((
int)vel);
123 KatanaControllerKni::final()
125 if( __active_motors.size() < 1 )
129 for(
unsigned int i=0; i<__active_motors.size(); i++) {
130 final &= motor_final(__active_motors.at(i));
132 cleanup_active_motors();
137 KatanaControllerKni::joint_angles()
142 KatanaControllerKni::joint_encoders()
148 KatanaControllerKni::calibrate()
151 __katana->calibrate();
158 KatanaControllerKni::stop()
161 __katana->freezeRobot();
168 KatanaControllerKni::turn_on()
171 __katana->switchRobotOn();
178 KatanaControllerKni::turn_off()
181 __katana->switchRobotOff();
188 KatanaControllerKni::read_coordinates(
bool refresh)
191 __katana->getCoordinates(__x, __y, __z, __phi, __theta, __psi, refresh);
198 KatanaControllerKni::read_motor_data()
201 if( __active_motors.size() == (
unsigned short)__katana->getNumberOfMotors() ) {
202 __katbase->recvMPS();
203 __katbase->recvGMS();
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();
216 KatanaControllerKni::read_sensor_data()
219 __sensor_ctrl->recvDAT();
220 }
catch (::ParameterReadingException &e) {
228 KatanaControllerKni::gripper_open(
bool blocking)
231 __katana->openGripper(blocking);
236 __active_motors.clear();
237 __active_motors.resize(1);
238 __active_motors[0] = __katbase->GetMOT()->cnt - 1;
240 __gripper_last_pos.clear();
241 __gripper_last_pos[0] = __katbase->GetMOT()->arr[__active_motors[0]].GetPVP()->pos;
242 __gripper_last_pos[1] = 0;
246 KatanaControllerKni::gripper_close(
bool blocking)
249 __katana->closeGripper(blocking);
254 __active_motors.clear();
255 __active_motors.resize(1);
256 __active_motors[0] = __katbase->GetMOT()->cnt - 1;
258 __gripper_last_pos.clear();
259 __gripper_last_pos[0] = __katbase->GetMOT()->arr[__active_motors[0]].GetPVP()->pos;
260 __gripper_last_pos[1] = 0;
264 KatanaControllerKni::move_to(
float x,
float y,
float z,
float phi,
float theta,
float psi,
bool blocking)
266 cleanup_active_motors();
269 __katana->moveRobotTo(__x, __y, __z, __phi, __theta, __psi, blocking);
270 }
catch (KNI::NoSolutionException &e) {
277 for(
short i=0; i<__katana->getNumberOfMotors(); ++i) {
283 KatanaControllerKni::move_to(std::vector<int> encoders,
bool blocking)
285 cleanup_active_motors();
288 __katana->moveRobotToEnc(encoders);
293 for(
unsigned short i=0; i<encoders.size(); ++i) {
299 KatanaControllerKni::move_to(std::vector<float> angles,
bool blocking)
301 std::vector<int> encoders;
304 for(
unsigned int i=0; i<angles.size(); i++) {
305 encoders.push_back(KNI_MHF::rad2enc( (
double)angles.at(i), __motor_init.at(i).angleOffset, __motor_init.at(i).encodersPerCycle, __motor_init.at(i).encoderOffset, __motor_init.at(i).rotationDirection));
311 move_to(encoders, blocking);
315 KatanaControllerKni::move_motor_to(
unsigned short id,
int enc,
bool blocking)
320 cleanup_active_motors();
323 __katana->moveMotorToEnc(
id, enc);
328 add_active_motor(
id);
332 KatanaControllerKni::move_motor_to(
unsigned short id,
float angle,
bool blocking)
337 cleanup_active_motors();
340 __katana->moveMotorTo(
id, angle);
345 add_active_motor(
id);
349 KatanaControllerKni::move_motor_by(
unsigned short id,
int enc,
bool blocking)
354 cleanup_active_motors();
357 __katana->moveMotorByEnc(
id, enc);
362 add_active_motor(
id);
366 KatanaControllerKni::move_motor_by(
unsigned short id,
float angle,
bool blocking)
371 cleanup_active_motors();
374 __katana->moveMotorBy(
id, angle);
379 add_active_motor(
id);
385 KatanaControllerKni::x()
391 KatanaControllerKni::y()
397 KatanaControllerKni::z()
403 KatanaControllerKni::phi()
409 KatanaControllerKni::theta()
415 KatanaControllerKni::psi()
421 KatanaControllerKni::get_sensors(std::vector<int>& to,
bool refresh)
427 const TSctDAT *sensor_data = __sensor_ctrl->GetDAT();
429 const int num_sensors = (size_t)sensor_data->cnt;
431 to.resize(num_sensors);
433 for (
int i = 0; i < num_sensors; ++i) {
434 to[i] = sensor_data->arr[i];
442 KatanaControllerKni::get_encoders(std::vector<int>& to,
bool refresh)
445 std::vector<int> encoders = __katana->getRobotEncoders(refresh);
455 KatanaControllerKni::get_angles(std::vector<float>& to,
bool refresh)
458 std::vector<int> encoders = __katana->getRobotEncoders(refresh);
461 for(
unsigned int i=0; i<encoders.size(); i++) {
462 to.push_back(KNI_MHF::enc2rad( encoders.at(i), __motor_init.at(i).angleOffset, __motor_init.at(i).encodersPerCycle, __motor_init.at(i).encoderOffset, __motor_init.at(i).rotationDirection));
471 KatanaControllerKni::motor_oor(
unsigned short id)
473 return id > (
unsigned short)__katana->getNumberOfMotors();
477 KatanaControllerKni::motor_final(
unsigned short id)
479 CMotBase mot = __katbase->GetMOT()->arr[id];
480 if (mot.GetPVP()->msf == MSF_MOTCRASHED)
484 unsigned short gripper_not_moved = 0;
485 if (
id == __katbase->GetMOT()->cnt - 1) {
486 if (__gripper_last_pos[0] == mot.GetPVP()->pos) {
487 __gripper_last_pos[1] += 1;
489 __gripper_last_pos[0] = mot.GetPVP()->pos;
490 __gripper_last_pos[1] = 0;
492 gripper_not_moved = __gripper_last_pos[1];
495 return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10)
496 or (gripper_not_moved > 3);
500 KatanaControllerKni::cleanup_active_motors()
502 for(
unsigned int i=0; i<__active_motors.size(); ++i) {
503 if( motor_final(__active_motors.at(i)) ) {
504 __active_motors.erase(__active_motors.begin()+i);
511 KatanaControllerKni::add_active_motor(
unsigned short id)
513 for(
unsigned int i=0; i<__active_motors.size(); ++i) {
514 if( __active_motors.at(i) == id ) {
518 __active_motors.push_back(
id);
At least one motor is out of range.
Fawkes library namespace.
No joint configuration for desired target found.
virtual const char * what() const
Get primary string.
Base class for exceptions in Fawkes.
At least one motor crashed.