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>
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());
uint32_t nr() const
Get nr value.
virtual void init()=0
Initialize controller.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
void set_sensor_value(unsigned int index, const uint8_t new_sensor_value)
Set sensor_value value at given index.
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.
bool finished() const
Did the motion finish already?
Base class for all messages passed through interfaces in Fawkes BlackBoard.
virtual double y()=0
Get y-coordinate of latest endeffector position.
virtual void loop()
Code to execute in the thread.
float angle() const
Get angle value.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void vlog_debug(const char *component, const char *format, va_list va)=0
Log debug message.
float rot_x() const
Get rot_x value.
uint32_t nr() const
Get nr value.
float z() const
Get z value.
float angle() const
Get angle value.
void set_z(const float new_z)
Set z value.
LinearGotoKniMessage Fawkes BlackBoard Interface Message.
float phi() const
Get phi value.
uint8_t max_velocity() const
Get max_velocity value.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
void set_y(const float new_y)
Set y value.
Fawkes library namespace.
Katana motor control thread.
CalibrateMessage Fawkes BlackBoard Interface Message.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void unlock()
Unlock the mutex.
virtual double theta()=0
Get theta-rotation of latest endeffector orientation.
bool is_straight() const
Get straight value.
void set_final(const bool new_final)
Set final value.
SetMotorEncoderMessage Fawkes BlackBoard Interface Message.
float theta() const
Get theta value.
uint32_t enc() const
Get enc value.
float offset_xy() const
Get offset_xy value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
A class for handling time.
size_t maxlenof_sensor_value() const
Get maximum length of sensor_value value.
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm...
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
SetMotorAngleMessage Fawkes BlackBoard Interface Message.
unsigned int id() const
Get message ID.
virtual double z()=0
Get z-coordinate of latest endeffector position.
void set_phi(const float new_phi)
Set phi value.
Base class for all Fawkes BlackBoard interfaces.
void set_psi(const float new_psi)
Set psi value.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
void set_theta(const float new_theta)
Set theta value.
virtual void read_coordinates(bool refresh=false)=0
Store current coordinates of endeeffctor.
virtual void turn_on()=0
Turn on arm/motors.
uint32_t nr() const
Get nr value.
Clock * clock
By means of this member access to the clock is given.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual void finalize()
Finalize the thread.
ObjectGotoMessage Fawkes BlackBoard Interface Message.
virtual void once()
Execute an action exactly once.
float z() const
Get z value.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
void msgq_pop()
Erase first message from queue.
virtual void init()
Initialize the thread.
char * trans_frame() const
Get trans_frame value.
float theta() const
Get theta value.
float y() const
Get y value.
OpenGripperMessage Fawkes BlackBoard Interface Message.
virtual void reset()
Reset for next execution.
virtual const char * what() const
Get primary string.
KatanaInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
void set_clock(Clock *clock)
Set clock for this instance.
LinearGotoMessage Fawkes BlackBoard Interface Message.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void finalize()
Finalize the thread.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
bool is_enabled() const
Get enabled value.
float y() const
Get y value.
float psi() const
Get psi value.
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm...
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual double x()=0
Get x-coordinate of latest endeffector position.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)=0
Get sensor values.
uint32_t nr() const
Get nr value.
Katana sensor acquisition thread.
void set_angles(unsigned int index, const float new_angles)
Set angles value at given index.
unsigned int error_code() const
Error code.
bool msgq_first_is()
Check if first message has desired type.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
class KatanaGotoOpenRaveThread
CloseGripperMessage Fawkes BlackBoard Interface Message.
float theta_error() const
Get theta_error value.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
void set_mode(gripper_mode_t mode)
Set mode.
const char * name() const
Get name of thread.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
void cancel()
Cancel a thread.
Wrapper class to add time stamp and frame ID to base types.
virtual void turn_off()=0
Turn off arm/motors.
void set_x(const float new_x)
Set x value.
StopMessage Fawkes BlackBoard Interface Message.
MoveMotorAngleMessage Fawkes BlackBoard Interface Message.
float x() const
Get x value.
void set_encoders(unsigned int index, const int32_t new_encoders)
Set encoders value at given index.
void update_sensor_values()
Update sensor values as necessary.
void set_time(const timeval *tv)
Sets the time.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float x() const
Get x value.
Katana linear goto thread.
float psi() const
Get psi value.
KatanaActThread()
Constructor.
void join()
Join the thread.
MoveMotorEncoderMessage Fawkes BlackBoard Interface Message.
char * plannerparams() const
Get plannerparams value.
virtual void set_angle(unsigned int nr, float value, bool inc=false)
Set target angle value.
virtual double psi()=0
Get psi-rotation of latest endeffector orientation.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
virtual bool joint_encoders()=0
Check if controller provides joint encoder values.
ParkMessage Fawkes BlackBoard Interface Message.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual void set_max_velocity(unsigned int vel)=0
Set maximum velocity.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
char * object() const
Get object value.
void set_max_velocity(const uint8_t new_max_velocity)
Set max_velocity value.
virtual double phi()=0
Get x-coordinate of latest endeffector position.
virtual void set_encoder(unsigned int nr, int value, bool inc=false)
Set target encoder value.
~KatanaActThread()
Destructor.
float phi() const
Get phi value.
FlushMessage Fawkes BlackBoard Interface Message.
Katana calibration thread.
Configuration * config
This is the Configuration member used to access the configuration.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
uint32_t enc() const
Get enc value.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
char * rot_frame() const
Get rot_frame value.
virtual void stop()=0
Stop movement immediately.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
void start(bool wait=true)
Call this method to start the thread.
SetEnabledMessage Fawkes BlackBoard Interface Message.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
float * angles() const
Get angles value.
virtual void close(Interface *interface)=0
Close interface.