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);
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.