23 #include "controller_kni.h"
25 #include "exception.h"
27 #include <common/MathHelperFunctions.h>
28 #include <core/exceptions/software.h>
42 : cfg_device_(
"/dev/ttyS0"), cfg_kni_conffile_(
"/etc/kni3/hd300/katana6M180.cfg")
44 cfg_read_timeout_ = 100;
45 cfg_write_timeout_ = 0;
47 gripper_last_pos_.clear();
48 gripper_last_pos_.resize(2);
51 phi_ = theta_ = psi_ = 0.;
72 std::string &kni_conffile,
73 unsigned int read_timeout,
74 unsigned int write_timeout)
77 cfg_kni_conffile_ = kni_conffile;
78 cfg_read_timeout_ = read_timeout;
79 cfg_write_timeout_ = write_timeout;
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()));
89 protocol_.reset(
new CCplSerialCRC());
90 protocol_->init(device_.get());
93 katana_->create(cfg_kni_conffile_.c_str(), protocol_.get());
94 katbase_ = katana_->GetBase();
95 sensor_ctrl_ = &katbase_->GetSCT()->arr[0];
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());
116 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()) {
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;
272 cleanup_active_motors();
275 katana_->moveRobotTo(x_, y_, z_, phi_, theta_, psi_, blocking);
276 }
catch (KNI::NoSolutionException &e) {
282 for (
short i = 0; i < katana_->getNumberOfMotors(); ++i) {
290 cleanup_active_motors();
293 katana_->moveRobotToEnc(encoders);
298 for (
unsigned short i = 0; i < encoders.size(); ++i) {
306 std::vector<int> encoders;
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));
329 cleanup_active_motors();
332 katana_->moveMotorToEnc(
id, enc);
337 add_active_motor(
id);
346 cleanup_active_motors();
349 katana_->moveMotorTo(
id, angle);
354 add_active_motor(
id);
363 cleanup_active_motors();
366 katana_->moveMotorByEnc(
id, enc);
371 add_active_motor(
id);
380 cleanup_active_motors();
383 katana_->moveMotorBy(
id, angle);
388 add_active_motor(
id);
435 const TSctDAT *sensor_data = sensor_ctrl_->GetDAT();
437 const int num_sensors = (size_t)sensor_data->cnt;
439 to.resize(num_sensors);
441 for (
int i = 0; i < num_sensors; ++i) {
442 to[i] = sensor_data->arr[i];
453 std::vector<int> encoders = katana_->getRobotEncoders(refresh);
466 std::vector<int> encoders = katana_->getRobotEncoders(refresh);
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));
482 KatanaControllerKni::motor_oor(
unsigned short id)
484 return id > (
unsigned short)katana_->getNumberOfMotors();
488 KatanaControllerKni::motor_final(
unsigned short id)
490 CMotBase mot = katbase_->GetMOT()->arr[id];
491 if (mot.GetPVP()->msf == MSF_MOTCRASHED)
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;
500 gripper_last_pos_[0] = mot.GetPVP()->pos;
501 gripper_last_pos_[1] = 0;
503 gripper_not_moved = gripper_last_pos_[1];
506 return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10) or (gripper_not_moved > 3);
510 KatanaControllerKni::cleanup_active_motors()
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);
521 KatanaControllerKni::add_active_motor(
unsigned short id)
523 for (
unsigned int i = 0; i < active_motors_.size(); ++i) {
524 if (active_motors_.at(i) ==
id) {
528 active_motors_.push_back(
id);
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
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.
KatanaControllerKni()
Constructor.
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.
No joint configuration for desired target found.
At least one motor is out of range.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Fawkes library namespace.