24 #include "act_thread.h"
26 #include "calib_thread.h"
27 #include "controller_kni.h"
28 #include "controller_openrave.h"
29 #include "goto_openrave_thread.h"
30 #include "goto_thread.h"
31 #include "gripper_thread.h"
32 #include "motion_thread.h"
33 #include "motor_control_thread.h"
34 #include "sensacq_thread.h"
36 #include <core/threading/mutex_locker.h>
37 #include <interfaces/JointInterface.h>
38 #include <interfaces/KatanaInterface.h>
39 #include <utils/math/angle.h>
40 #include <utils/time/time.h>
43 # include <plugins/openrave/manipulator.h>
44 # include <plugins/openrave/robot.h>
53 using namespace fawkes::tf;
65 :
Thread(
"KatanaActThread",
Thread::OPMODE_WAITFORWAKEUP),
70 last_update_ =
new Time();
86 std::string cfg_prefix =
"/hardware/katana/";
89 cfg_kni_conffile_ =
config->
get_string((cfg_prefix +
"kni_conffile").c_str());
90 cfg_auto_calibrate_ =
config->
get_bool((cfg_prefix +
"auto_calibrate").c_str());
91 cfg_defmax_speed_ =
config->
get_uint((cfg_prefix +
"default_max_speed").c_str());
92 cfg_read_timeout_ =
config->
get_uint((cfg_prefix +
"read_timeout_msec").c_str());
93 cfg_write_timeout_ =
config->
get_uint((cfg_prefix +
"write_timeout_msec").c_str());
94 cfg_gripper_pollint_ =
config->
get_uint((cfg_prefix +
"gripper_pollint_msec").c_str());
95 cfg_goto_pollint_ =
config->
get_uint((cfg_prefix +
"goto_pollint_msec").c_str());
101 cfg_park_theta_ =
config->
get_float((cfg_prefix +
"park_theta").c_str());
104 cfg_distance_scale_ =
config->
get_float((cfg_prefix +
"distance_scale").c_str());
106 cfg_update_interval_ =
config->
get_float((cfg_prefix +
"update_interval").c_str());
109 cfg_frame_gripper_ =
config->
get_string((cfg_prefix +
"frame/gripper").c_str());
110 cfg_frame_openrave_ =
config->
get_string((cfg_prefix +
"frame/openrave").c_str());
113 cfg_OR_enabled_ =
config->
get_bool((cfg_prefix +
"openrave/enabled").c_str());
114 cfg_OR_use_viewer_ =
config->
get_bool((cfg_prefix +
"openrave/use_viewer").c_str());
115 cfg_OR_auto_load_ik_ =
config->
get_bool((cfg_prefix +
"openrave/auto_load_ik").c_str());
116 cfg_OR_robot_file_ =
config->
get_string((cfg_prefix +
"openrave/robot_file").c_str());
117 cfg_OR_arm_model_ =
config->
get_string((cfg_prefix +
"openrave/arm_model").c_str());
119 cfg_OR_enabled_ =
false;
123 std::string joint_name;
124 for (
long long i = 0; i < 5; ++i) {
125 joint_name =
config->
get_string((cfg_prefix +
"joints/" + std::to_string(i)).c_str());
137 if (cfg_controller_ ==
"kni") {
141 kat_ctrl->
setup(cfg_device_, cfg_kni_conffile_, cfg_read_timeout_, cfg_write_timeout_);
147 }
else if (cfg_controller_ ==
"openrave") {
149 if (!cfg_OR_enabled_) {
151 "Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!");
155 throw fawkes::Exception(
"Cannot use controller 'openrave', OpenRAVE not installed!");
159 throw fawkes::Exception(
"Invalid controller given: '%s'", cfg_controller_.c_str());
165 joint_ifs_ =
new std::vector<JointInterface *>();
167 for (
long long i = 0; i < 5; ++i) {
168 joint_name =
config->
get_string((cfg_prefix +
"joints/" + std::to_string(i)).c_str());
170 joint_ifs_->push_back(joint_if);
176 joint_ifs_->push_back(joint_if);
180 joint_ifs_->push_back(joint_if);
202 cfg_OR_auto_load_ik_,
204 if (cfg_OR_enabled_) {
205 goto_openrave_thread_->init();
220 sensacq_thread_->
start();
225 #ifdef USE_TIMETRACKER
228 ttc_read_sensor_ = tt_->add_class(
"Read Sensor");
236 if (actmot_thread_) {
238 actmot_thread_->
join();
239 actmot_thread_ = NULL;
246 sensacq_thread_->
cancel();
247 sensacq_thread_->
join();
248 sensacq_thread_.
reset();
254 calib_thread_ = NULL;
256 gripper_thread_ = NULL;
257 motor_control_thread_ = NULL;
259 if (cfg_OR_enabled_ && goto_openrave_thread_)
261 goto_openrave_thread_ = NULL;
279 for (std::vector<JointInterface *>::iterator it = joint_ifs_->begin(); it != joint_ifs_->end();
288 if (cfg_auto_calibrate_) {
289 start_motion(calib_thread_, 0,
"Auto calibration enabled, calibrating");
299 KatanaActThread::update_position(
bool refresh)
303 if (cfg_controller_ ==
"kni") {
304 katana_if_->
set_x(cfg_distance_scale_ * katana_->
x());
305 katana_if_->
set_y(cfg_distance_scale_ * katana_->
y());
306 katana_if_->
set_z(cfg_distance_scale_ * katana_->
z());
307 }
else if (cfg_controller_ ==
"openrave") {
310 "tf frames not existing: '%s'. Skipping transform to kni coordinates.",
311 cfg_frame_openrave_.c_str());
316 cfg_frame_openrave_);
320 katana_if_->
set_x(target.getX());
321 katana_if_->
set_y(target.getY());
322 katana_if_->
set_z(target.getZ());
332 float *a = katana_if_->
angles();
334 joint_ifs_->at(0)->set_position(a[0] + M_PI);
335 joint_ifs_->at(1)->set_position(a[1]);
336 joint_ifs_->at(2)->set_position(a[2] + M_PI);
337 joint_ifs_->at(3)->set_position(a[3] - M_PI);
338 joint_ifs_->at(4)->set_position(a[4]);
339 joint_ifs_->at(5)->set_position(-a[5] / 2.f - 0.5f);
340 joint_ifs_->at(6)->set_position(-a[5] / 2.f - 0.5f);
341 for (
unsigned int i = 0; i < joint_ifs_->size(); ++i) {
342 joint_ifs_->at(i)->write();
375 if (actmot_thread_ != calib_thread_) {
376 update_sensors(!actmot_thread_);
384 KatanaActThread::update_sensors(
bool refresh)
387 std::vector<int> sensors;
391 for (
int i = 0; i < num_sensors; ++i) {
392 if (sensors.at(i) <= 0) {
394 }
else if (sensors.at(i) >= 255) {
405 sensacq_thread_->
wakeup();
412 KatanaActThread::update_motors(
bool refresh)
416 std::vector<int> encoders;
418 for (
unsigned int i = 0; i < encoders.size(); i++) {
424 std::vector<float> angles;
426 for (
unsigned int i = 0; i < angles.size(); i++) {
447 va_start(arg, logmsg);
450 actmot_thread_ = motion_thread;
451 actmot_thread_->
start(
false);
459 KatanaActThread::stop_motion()
463 if (actmot_thread_) {
465 actmot_thread_->
join();
466 actmot_thread_ = NULL;
479 if (actmot_thread_) {
480 update_motors(
false);
481 update_position(
false);
487 actmot_thread_->
join();
490 if (actmot_thread_ == calib_thread_) {
493 actmot_thread_->
reset();
494 actmot_thread_ = NULL;
498 update_motors(
true);
499 update_position(
true);
502 if (cfg_OR_enabled_) {
503 goto_openrave_thread_->update_openrave_data();
508 update_position(
true);
509 update_motors(
true);
514 if ((now - last_update_) >= cfg_update_interval_) {
515 last_update_->
stamp();
516 update_position(
false);
517 update_motors(
false);
521 while (!katana_if_->
msgq_empty() && !actmot_thread_) {
524 start_motion(calib_thread_, msg->
id(),
"Calibrating arm");
531 if (!trans_frame_exists || !rot_frame_exists) {
533 "tf frames not existing: '%s%s%s'. Skipping message.",
535 trans_frame_exists ?
"" :
"', '",
536 rot_frame_exists ?
"" : msg->
rot_frame());
542 if (cfg_OR_enabled_) {
546 Vector3 offset(target.getX(), target.getY(), 0.f);
547 offset = (offset / offset.length())
552 if (strcmp(msg->
trans_frame(), cfg_frame_gripper_.c_str()) == 0) {
553 goto_openrave_thread_->set_target(
554 msg->
x(), msg->
y(), msg->
z(), msg->
phi(), msg->
theta(), msg->
psi());
555 goto_openrave_thread_->set_arm_extension(
true);
557 goto_openrave_thread_->set_target(
558 target.getX(), target.getY(), target.getZ(), msg->
phi(), msg->
theta(), msg->
psi());
560 goto_openrave_thread_->set_theta_error(msg->
theta_error());
561 goto_openrave_thread_->set_move_straight(msg->
is_straight());
562 # ifdef EARLY_PLANNING
563 goto_openrave_thread_->plan_target();
566 goto_openrave_thread_,
568 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u",
575 cfg_frame_openrave_.c_str(),
582 Vector3 offset(target.getX(), target.getY(), 0.f);
583 offset = (offset / offset.length())
587 goto_thread_->
set_target(target.getX() / cfg_distance_scale_,
588 target.getY() / cfg_distance_scale_,
589 target.getZ() / cfg_distance_scale_,
593 start_motion(goto_thread_,
595 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
602 cfg_frame_kni_.c_str());
609 float x = msg->
x() * cfg_distance_scale_;
610 float y = msg->
y() * cfg_distance_scale_;
611 float z = msg->
z() * cfg_distance_scale_;
616 if (cfg_OR_enabled_) {
619 goto_openrave_thread_->set_target(
620 target.getX(), target.getY(), target.getZ(), msg->
phi(), msg->
theta(), msg->
psi());
621 # ifdef EARLY_PLANNING
622 goto_openrave_thread_->plan_target();
624 start_motion(goto_openrave_thread_,
626 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
633 cfg_frame_openrave_.c_str());
637 msg->
x(), msg->
y(), msg->
z(), msg->
phi(), msg->
theta(), msg->
psi());
639 start_motion(goto_thread_,
641 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
648 cfg_frame_kni_.c_str());
657 rot_x = msg->
rot_x();
660 goto_openrave_thread_->set_target(msg->
object(), rot_x);
661 # ifdef EARLY_PLANNING
662 goto_openrave_thread_->plan_target();
664 start_motion(goto_openrave_thread_,
666 "Linear movement to object (%s, %f)",
674 if (cfg_OR_enabled_) {
678 cfg_park_y_ * cfg_distance_scale_,
679 cfg_park_z_ * cfg_distance_scale_),
683 goto_openrave_thread_->set_target(target.getX(),
689 # ifdef EARLY_PLANNING
690 goto_openrave_thread_->plan_target();
692 start_motion(goto_openrave_thread_, msg->
id(),
"Parking arm");
696 cfg_park_x_, cfg_park_y_, cfg_park_z_, cfg_park_phi_, cfg_park_theta_, cfg_park_psi_);
697 start_motion(goto_thread_, msg->
id(),
"Parking arm");
703 start_motion(gripper_thread_, msg->
id(),
"Opening gripper");
708 start_motion(gripper_thread_, msg->
id(),
"Closing gripper");
717 update_position(
true);
718 update_motors(
true);
721 goto_openrave_thread_->update_openrave_data();
737 max_vel = cfg_defmax_speed_;
749 if (cfg_OR_enabled_) {
751 goto_openrave_thread_->set_plannerparams(msg->
plannerparams());
759 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
765 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
771 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
777 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
788 #ifdef USE_TIMETRACKER
789 if (++tt_count_ > 100) {
791 tt_->print_to_stdout();
804 logger->
log_info(name(),
"Flushing message queue");
805 katana_if_->msgq_flush();
808 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());