23 #include "controller_kni.h" 24 #include "exception.h" 26 #include <core/exceptions/software.h> 29 #include <common/MathHelperFunctions.h> 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);
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;
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());
115 __katana->setRobotVelocityLimit((
int)vel);
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();
151 __katana->calibrate();
161 __katana->freezeRobot();
171 __katana->switchRobotOn();
181 __katana->switchRobotOff();
191 __katana->getCoordinates(__x, __y, __z, __phi, __theta, __psi, refresh);
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();
219 __sensor_ctrl->recvDAT();
220 }
catch (::ParameterReadingException &e) {
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;
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;
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) {
285 cleanup_active_motors();
288 __katana->moveRobotToEnc(encoders);
293 for(
unsigned short i=0; i<encoders.size(); ++i) {
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));
320 cleanup_active_motors();
323 __katana->moveMotorToEnc(
id, enc);
328 add_active_motor(
id);
337 cleanup_active_motors();
340 __katana->moveMotorTo(
id, angle);
345 add_active_motor(
id);
354 cleanup_active_motors();
357 __katana->moveMotorByEnc(
id, enc);
362 add_active_motor(
id);
371 cleanup_active_motors();
374 __katana->moveMotorBy(
id, angle);
379 add_active_motor(
id);
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];
445 std::vector<int> encoders = __katana->getRobotEncoders(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);
virtual double z()
Get z-coordinate of latest endeffector position.
virtual bool final()
Check if movement is final.
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 double theta()
Get theta-rotation of latest endeffector orientation.
virtual void read_sensor_data()
Read all sensor data from device into controller libray.
At least one motor is out of range.
Fawkes library namespace.
virtual void turn_on()
Turn on arm/motors.
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 double phi()
Get x-coordinate of latest endeffector position.
virtual double x()
Get x-coordinate of latest endeffector position.
virtual const char * what() const
Get primary string.
No joint configuration for desired target found.
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 void turn_off()
Turn off arm/motors.
virtual void move_motor_to(unsigned short id, int enc, bool blocking=false)
Move single joint/motor to encoder value.
virtual void set_max_velocity(unsigned int vel)
Set maximum velocity.
virtual void gripper_close(bool blocking=false)
Close Gripper.
virtual double y()
Get y-coordinate of latest endeffector position.
virtual void read_motor_data()
Read motor data of currently active joints from device into controller libray.
virtual bool joint_angles()
Check if controller provides joint angle values.
virtual ~KatanaControllerKni()
Destructor.
virtual void calibrate()
Calibrate the arm.
KatanaControllerKni()
Constructor.
Base class for exceptions in Fawkes.
virtual double psi()
Get psi-rotation of latest endeffector orientation.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)
Get encoder values of joints/motors.
virtual void get_angles(std::vector< float > &to, bool refresh=false)
Get angle values of joints/motors.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)
Get sensor values.
virtual bool joint_encoders()
Check if controller provides joint encoder values.
At least one motor crashed.
virtual void gripper_open(bool blocking=false)
Open Gripper.
virtual void stop()
Stop movement immediately.
virtual void read_coordinates(bool refresh=false)
Store current coordinates of endeeffctor.