23 #include "act_thread.h"
24 #include "controller_kni.h"
25 #include "controller_openrave.h"
27 #include <core/threading/mutex_locker.h>
28 #include <interfaces/KatanaInterface.h>
29 #include <utils/math/angle.h>
30 #include <utils/time/time.h>
35 using namespace fawkes;
37 using namespace fawkes::tf;
49 :
Thread(
"KatanaActThread",
Thread::OPMODE_WAITFORWAKEUP),
54 __last_update =
new Time();
74 __cfg_auto_calibrate =
config->
get_bool(
"/hardware/katana/auto_calibrate");
75 __cfg_defmax_speed =
config->
get_uint(
"/hardware/katana/default_max_speed");
76 __cfg_read_timeout =
config->
get_uint(
"/hardware/katana/read_timeout_msec");
77 __cfg_write_timeout =
config->
get_uint(
"/hardware/katana/write_timeout_msec");
78 __cfg_gripper_pollint =
config->
get_uint(
"/hardware/katana/gripper_pollint_msec");
79 __cfg_goto_pollint =
config->
get_uint(
"/hardware/katana/goto_pollint_msec");
88 __cfg_distance_scale =
config->
get_float(
"/hardware/katana/distance_scale");
90 __cfg_update_interval =
config->
get_float(
"/hardware/katana/update_interval");
92 __cfg_frame_kni =
config->
get_string(
"/plugins/static-transforms/transforms/katana_kni/child_frame");
93 __cfg_frame_openrave =
config->
get_string(
"/plugins/static-transforms/transforms/openrave/child_frame");
96 __cfg_OR_enabled =
config->
get_bool(
"/hardware/katana/openrave/enabled");
97 __cfg_OR_use_viewer =
config->
get_bool(
"/hardware/katana/openrave/use_viewer");
98 __cfg_OR_auto_load_ik =
config->
get_bool(
"/hardware/katana/openrave/auto_load_ik");
99 __cfg_OR_robot_file =
config->
get_string(
"/hardware/katana/openrave/robot_file");
100 __cfg_OR_arm_model =
config->
get_string(
"/hardware/katana/openrave/arm_model");
102 __cfg_OR_enabled =
false;
109 if( __cfg_controller ==
"kni") {
113 kat_ctrl->
setup(__cfg_device, __cfg_kni_conffile,
114 __cfg_read_timeout, __cfg_write_timeout);
120 }
else if( __cfg_controller ==
"openrave") {
122 if(!__cfg_OR_enabled) {
123 throw fawkes::Exception(
"Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!");
127 throw fawkes::Exception(
"Cannot use controller 'openrave', OpenRAVE not installed!");
131 throw fawkes::Exception(
"Invalid controller given: '%s'", __cfg_controller.c_str());
141 __cfg_gripper_pollint);
146 __cfg_OR_arm_model, __cfg_OR_auto_load_ik, __cfg_OR_use_viewer);
148 {__goto_openrave_thread->init();}
162 __sensacq_thread->start();
167 #ifdef USE_TIMETRACKER
170 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
179 if ( __actmot_thread ) {
180 __actmot_thread->
cancel();
181 __actmot_thread->
join();
182 __actmot_thread = NULL;
184 __sensacq_thread->cancel();
185 __sensacq_thread->join();
186 __sensacq_thread.reset();
189 __calib_thread = NULL;
190 __goto_thread = NULL;
191 __gripper_thread = NULL;
192 __motor_control_thread = NULL;
195 {__goto_openrave_thread->
finalize();}
196 __goto_openrave_thread = NULL;
214 if ( __cfg_auto_calibrate ) {
215 start_motion(__calib_thread, 0,
"Auto calibration enabled, calibrating");
217 __katana_if->
write();
226 KatanaActThread::update_position(
bool refresh)
230 if( __cfg_controller ==
"kni") {
231 __katana_if->
set_x(__cfg_distance_scale * __katana->
x());
232 __katana_if->
set_y(__cfg_distance_scale * __katana->
y());
233 __katana_if->
set_z(__cfg_distance_scale * __katana->
z());
234 }
else if( __cfg_controller ==
"openrave") {
237 logger->
log_warn(
name(),
"tf frames not existing: '%s'. Skipping transform to kni coordinates.",
238 __cfg_frame_openrave.c_str() );
246 __katana_if->
set_x(target.getX());
247 __katana_if->
set_y(target.getY());
248 __katana_if->
set_z(target.getZ());
258 float *a = __katana_if->
angles();
261 static const float p90 =
deg2rad(90);
262 static const float p180 =
deg2rad(180);
264 Transform bs_j1(Quaternion(a[0], 0, 0), Vector3(0, 0, 0.141));
265 Transform j1_j2(Quaternion(0, a[1] - p90, 0), Vector3(0, 0, 0.064));
266 Transform j2_j3(Quaternion(0, a[2] + p180, 0), Vector3(0, 0, 0.190));
267 Transform j3_j4(Quaternion(0, -a[3] - p180, 0), Vector3(0, 0, 0.139));
268 Transform j4_j5(Quaternion(-a[4], 0, 0), Vector3(0, 0, 0.120));
269 Transform j5_gr(Quaternion(0, -p90, 0), Vector3(0, 0, 0.065));
289 if ( __actmot_thread != __calib_thread ) {
290 update_sensors(! __actmot_thread);
299 KatanaActThread::update_sensors(
bool refresh)
302 std::vector<int> sensors;
306 for (
int i = 0; i < num_sensors; ++i) {
307 if (sensors.at(i) <= 0) {
309 }
else if (sensors.at(i) >= 255) {
319 if (refresh) __sensacq_thread->wakeup();
327 KatanaActThread::update_motors(
bool refresh)
331 std::vector<int> encoders;
333 for(
unsigned int i=0; i<encoders.size(); i++) {
339 std::vector<float> angles;
341 for(
unsigned int i=0; i<angles.size(); i++) {
358 unsigned int msgid,
const char *logmsg, ...)
361 va_start(arg, logmsg);
363 __sensacq_thread->set_enabled(
false);
364 __actmot_thread = motion_thread;
365 __actmot_thread->
start(
false);
374 KatanaActThread::stop_motion()
378 if (__actmot_thread) {
379 __actmot_thread->
cancel();
380 __actmot_thread->
join();
381 __actmot_thread = NULL;
395 if ( __actmot_thread ) {
396 update_motors(
false);
397 update_position(
false);
398 __katana_if->
write();
399 if (! __actmot_thread->
finished()) {
403 __actmot_thread->
join();
406 if (__actmot_thread == __calib_thread) {
409 __actmot_thread->
reset();
410 __actmot_thread = NULL;
412 __sensacq_thread->set_enabled(
true);
414 update_motors(
true);
415 update_position(
true);
418 if(__cfg_OR_enabled) { __goto_openrave_thread->update_openrave_data(); }
422 update_position(
true);
423 update_motors(
true);
428 if ((now - __last_update) >= __cfg_update_interval) {
429 __last_update->
stamp();
430 update_position(
false);
431 update_motors(
false);
435 while (! __katana_if->
msgq_empty() && ! __actmot_thread ) {
438 start_motion(__calib_thread, msg->
id(),
"Calibrating arm");
445 if( !trans_frame_exists || !rot_frame_exists ) {
448 trans_frame_exists ?
"" :
"', '",
449 rot_frame_exists ?
"" : msg->
rot_frame() );
454 if( __cfg_OR_enabled ) {
458 Vector3 offset(target.getX(), target.getY(), 0.f);
459 offset = (offset/offset.length()) * msg->
offset_xy();
463 if( strcmp(msg->
trans_frame(),
"/katana/gripper")==0 ) {
464 __goto_openrave_thread->set_target(msg->
x(), msg->
y(), msg->
z(),
466 __goto_openrave_thread->set_arm_extension(
true);
468 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
471 __goto_openrave_thread->set_theta_error(msg->
theta_error());
472 __goto_openrave_thread->set_move_straight(msg->
is_straight());
473 #ifdef EARLY_PLANNING
474 __goto_openrave_thread->plan_target();
476 start_motion(__goto_openrave_thread, msg->
id(),
477 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u",
478 target.getX(), target.getY(), target.getZ(),
484 Vector3 offset(target.getX(), target.getY(), 0.f);
485 offset = (offset/offset.length()) * msg->
offset_xy();
488 __goto_thread->
set_target(target.getX() / __cfg_distance_scale,
489 target.getY() / __cfg_distance_scale,
490 target.getZ() / __cfg_distance_scale,
492 start_motion(__goto_thread, msg->
id(),
493 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
494 target.getX(), target.getY(), target.getZ(),
495 msg->
phi(), msg->
theta(), msg->
psi(), __cfg_frame_kni.c_str());
502 float x = msg->
x() * __cfg_distance_scale;
503 float y = msg->
y() * __cfg_distance_scale;
504 float z = msg->
z() * __cfg_distance_scale;
510 if( __cfg_OR_enabled ) {
513 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
515 #ifdef EARLY_PLANNING
516 __goto_openrave_thread->plan_target();
518 start_motion(__goto_openrave_thread, msg->
id(),
519 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
520 target.getX(), target.getY(), target.getZ(),
521 msg->
phi(), msg->
theta(), msg->
psi(), __cfg_frame_openrave.c_str());
527 start_motion(__goto_thread, msg->
id(),
528 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
530 msg->
phi(), msg->
theta(), msg->
psi(), __cfg_frame_kni.c_str());
539 { rot_x = msg->
rot_x(); }
541 __goto_openrave_thread->set_target(msg->
object(), rot_x);
542 #ifdef EARLY_PLANNING
543 __goto_openrave_thread->plan_target();
545 start_motion(__goto_openrave_thread, msg->
id(),
546 "Linear movement to object (%s, %f)", msg->
object(), msg->
rot_x());
552 if(__cfg_OR_enabled) {
556 __cfg_park_y * __cfg_distance_scale,
557 __cfg_park_z * __cfg_distance_scale),
561 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
562 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
563 #ifdef EARLY_PLANNING
564 __goto_openrave_thread->plan_target();
566 start_motion(__goto_openrave_thread, msg->
id(),
"Parking arm");
569 __goto_thread->
set_target(__cfg_park_x, __cfg_park_y, __cfg_park_z,
570 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
571 start_motion(__goto_thread, msg->
id(),
"Parking arm");
577 start_motion(__gripper_thread, msg->
id(),
"Opening gripper");
582 start_motion(__gripper_thread, msg->
id(),
"Closing gripper");
591 update_position(
true);
592 update_motors(
true);
595 {__goto_openrave_thread->update_openrave_data();}
610 if ( max_vel == 0 ) max_vel = __cfg_defmax_speed;
622 if( __cfg_OR_enabled ) {
624 __goto_openrave_thread->set_plannerparams(msg->
plannerparams());
632 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
638 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
644 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
650 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
659 __katana_if->
write();
661 #ifdef USE_TIMETRACKER
662 if (++__tt_count > 100) {
664 __tt->print_to_stdout();
679 logger->
log_info(name(),
"Flushing message queue");
680 __katana_if->msgq_flush();
683 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());