24 #include "act_thread.h" 25 #include "motion_thread.h" 26 #include "calib_thread.h" 27 #include "goto_thread.h" 28 #include "goto_openrave_thread.h" 29 #include "gripper_thread.h" 30 #include "sensacq_thread.h" 31 #include "motor_control_thread.h" 32 #include "controller_kni.h" 33 #include "controller_openrave.h" 35 #include <core/threading/mutex_locker.h> 36 #include <interfaces/KatanaInterface.h> 37 #include <interfaces/JointInterface.h> 38 #include <utils/math/angle.h> 39 #include <utils/time/time.h> 42 #include <plugins/openrave/robot.h> 43 #include <plugins/openrave/manipulator.h> 63 :
Thread(
"KatanaActThread",
Thread::OPMODE_WAITFORWAKEUP),
68 __last_update =
new Time();
85 std::string cfg_prefix =
"/hardware/katana/";
88 __cfg_kni_conffile =
config->
get_string((cfg_prefix +
"kni_conffile").c_str());
89 __cfg_auto_calibrate =
config->
get_bool((cfg_prefix +
"auto_calibrate").c_str());
90 __cfg_defmax_speed =
config->
get_uint((cfg_prefix +
"default_max_speed").c_str());
91 __cfg_read_timeout =
config->
get_uint((cfg_prefix +
"read_timeout_msec").c_str());
92 __cfg_write_timeout =
config->
get_uint((cfg_prefix +
"write_timeout_msec").c_str());
93 __cfg_gripper_pollint =
config->
get_uint((cfg_prefix +
"gripper_pollint_msec").c_str());
94 __cfg_goto_pollint =
config->
get_uint((cfg_prefix +
"goto_pollint_msec").c_str());
100 __cfg_park_theta =
config->
get_float((cfg_prefix +
"park_theta").c_str());
103 __cfg_distance_scale =
config->
get_float((cfg_prefix +
"distance_scale").c_str());
105 __cfg_update_interval =
config->
get_float((cfg_prefix +
"update_interval").c_str());
108 __cfg_frame_gripper =
config->
get_string((cfg_prefix +
"frame/gripper").c_str());
109 __cfg_frame_openrave =
config->
get_string((cfg_prefix +
"frame/openrave").c_str());
112 __cfg_OR_enabled =
config->
get_bool((cfg_prefix +
"openrave/enabled").c_str());
113 __cfg_OR_use_viewer =
config->
get_bool((cfg_prefix +
"openrave/use_viewer").c_str());
114 __cfg_OR_auto_load_ik =
config->
get_bool((cfg_prefix +
"openrave/auto_load_ik").c_str());
115 __cfg_OR_robot_file =
config->
get_string((cfg_prefix +
"openrave/robot_file").c_str());
116 __cfg_OR_arm_model =
config->
get_string((cfg_prefix +
"openrave/arm_model").c_str());
118 __cfg_OR_enabled =
false;
122 std::string joint_name;
123 for(
long long i = 0; i < 5; ++i) {
124 joint_name =
config->
get_string((cfg_prefix +
"joints/" + std::to_string(i)).c_str() );
136 if( __cfg_controller ==
"kni") {
140 kat_ctrl->
setup(__cfg_device, __cfg_kni_conffile,
141 __cfg_read_timeout, __cfg_write_timeout);
147 }
else if( __cfg_controller ==
"openrave") {
149 if(!__cfg_OR_enabled) {
150 throw fawkes::Exception(
"Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!");
154 throw fawkes::Exception(
"Cannot use controller 'openrave', OpenRAVE not installed!");
158 throw fawkes::Exception(
"Invalid controller given: '%s'", __cfg_controller.c_str());
164 __joint_ifs =
new std::vector<JointInterface*>();
166 for(
long long i = 0; i < 5; ++i) {
167 joint_name =
config->
get_string((cfg_prefix +
"joints/" + std::to_string(i)).c_str() );
169 __joint_ifs->push_back( joint_if );
175 __joint_ifs->push_back( joint_if );
179 __joint_ifs->push_back( joint_if );
196 __cfg_OR_arm_model, __cfg_OR_auto_load_ik, __cfg_OR_use_viewer);
198 {__goto_openrave_thread->init();}
212 __sensacq_thread->
start();
217 #ifdef USE_TIMETRACKER 220 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
230 if( __actmot_thread ) {
231 __actmot_thread->
cancel();
232 __actmot_thread->
join();
233 __actmot_thread = NULL;
240 __sensacq_thread->
cancel();
241 __sensacq_thread->
join();
242 __sensacq_thread.
reset();
248 __calib_thread = NULL;
249 __goto_thread = NULL;
250 __gripper_thread = NULL;
251 __motor_control_thread = NULL;
253 if( __cfg_OR_enabled && __goto_openrave_thread )
255 __goto_openrave_thread = NULL;
273 for (std::vector<JointInterface*>::iterator it = __joint_ifs->begin(); it != __joint_ifs->end(); ++it) {
282 if ( __cfg_auto_calibrate ) {
283 start_motion(__calib_thread, 0,
"Auto calibration enabled, calibrating");
285 __katana_if->
write();
294 KatanaActThread::update_position(
bool refresh)
298 if( __cfg_controller ==
"kni") {
299 __katana_if->
set_x(__cfg_distance_scale * __katana->
x());
300 __katana_if->
set_y(__cfg_distance_scale * __katana->
y());
301 __katana_if->
set_z(__cfg_distance_scale * __katana->
z());
302 }
else if( __cfg_controller ==
"openrave") {
305 logger->
log_warn(
name(),
"tf frames not existing: '%s'. Skipping transform to kni coordinates.",
306 __cfg_frame_openrave.c_str() );
314 __katana_if->
set_x(target.getX());
315 __katana_if->
set_y(target.getY());
316 __katana_if->
set_z(target.getZ());
326 float *a = __katana_if->
angles();
328 __joint_ifs->at(0)->set_position( a[0] + M_PI);
329 __joint_ifs->at(1)->set_position( a[1]);
330 __joint_ifs->at(2)->set_position( a[2] + M_PI);
331 __joint_ifs->at(3)->set_position( a[3] - M_PI);
332 __joint_ifs->at(4)->set_position( a[4]);
333 __joint_ifs->at(5)->set_position( -a[5]/2.f - 0.5f);
334 __joint_ifs->at(6)->set_position( -a[5]/2.f - 0.5f);
335 for(
unsigned int i=0; i<__joint_ifs->size(); ++i) {
336 __joint_ifs->at(i)->write();
370 if ( __actmot_thread != __calib_thread ) {
371 update_sensors(! __actmot_thread);
380 KatanaActThread::update_sensors(
bool refresh)
383 std::vector<int> sensors;
387 for (
int i = 0; i < num_sensors; ++i) {
388 if (sensors.at(i) <= 0) {
390 }
else if (sensors.at(i) >= 255) {
400 if (refresh) __sensacq_thread->
wakeup();
408 KatanaActThread::update_motors(
bool refresh)
412 std::vector<int> encoders;
414 for(
unsigned int i=0; i<encoders.size(); i++) {
420 std::vector<float> angles;
422 for(
unsigned int i=0; i<angles.size(); i++) {
439 unsigned int msgid,
const char *logmsg, ...)
442 va_start(arg, logmsg);
445 __actmot_thread = motion_thread;
446 __actmot_thread->
start(
false);
455 KatanaActThread::stop_motion()
459 if (__actmot_thread) {
460 __actmot_thread->
cancel();
461 __actmot_thread->
join();
462 __actmot_thread = NULL;
476 if ( __actmot_thread ) {
477 update_motors(
false);
478 update_position(
false);
479 __katana_if->
write();
480 if (! __actmot_thread->
finished()) {
484 __actmot_thread->
join();
487 if (__actmot_thread == __calib_thread) {
490 __actmot_thread->
reset();
491 __actmot_thread = NULL;
495 update_motors(
true);
496 update_position(
true);
499 if(__cfg_OR_enabled) { __goto_openrave_thread->update_openrave_data(); }
503 update_position(
true);
504 update_motors(
true);
509 if ((now - __last_update) >= __cfg_update_interval) {
510 __last_update->
stamp();
511 update_position(
false);
512 update_motors(
false);
516 while (! __katana_if->
msgq_empty() && ! __actmot_thread ) {
519 start_motion(__calib_thread, msg->
id(),
"Calibrating arm");
526 if( !trans_frame_exists || !rot_frame_exists ) {
529 trans_frame_exists ?
"" :
"', '",
530 rot_frame_exists ?
"" : msg->
rot_frame() );
535 if( __cfg_OR_enabled ) {
539 Vector3 offset(target.getX(), target.getY(), 0.f);
540 offset = (offset/offset.length()) * msg->
offset_xy();
544 if( strcmp(msg->
trans_frame(), __cfg_frame_gripper.c_str())==0 ) {
545 __goto_openrave_thread->set_target(msg->
x(), msg->
y(), msg->
z(),
547 __goto_openrave_thread->set_arm_extension(
true);
549 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
552 __goto_openrave_thread->set_theta_error(msg->
theta_error());
553 __goto_openrave_thread->set_move_straight(msg->
is_straight());
554 #ifdef EARLY_PLANNING 555 __goto_openrave_thread->plan_target();
557 start_motion(__goto_openrave_thread, msg->
id(),
558 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u",
559 target.getX(), target.getY(), target.getZ(),
565 Vector3 offset(target.getX(), target.getY(), 0.f);
566 offset = (offset/offset.length()) * msg->
offset_xy();
569 __goto_thread->
set_target(target.getX() / __cfg_distance_scale,
570 target.getY() / __cfg_distance_scale,
571 target.getZ() / __cfg_distance_scale,
573 start_motion(__goto_thread, msg->
id(),
574 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
575 target.getX(), target.getY(), target.getZ(),
576 msg->
phi(), msg->
theta(), msg->
psi(), __cfg_frame_kni.c_str());
583 float x = msg->
x() * __cfg_distance_scale;
584 float y = msg->
y() * __cfg_distance_scale;
585 float z = msg->
z() * __cfg_distance_scale;
591 if( __cfg_OR_enabled ) {
594 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
596 #ifdef EARLY_PLANNING 597 __goto_openrave_thread->plan_target();
599 start_motion(__goto_openrave_thread, msg->
id(),
600 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
601 target.getX(), target.getY(), target.getZ(),
602 msg->
phi(), msg->
theta(), msg->
psi(), __cfg_frame_openrave.c_str());
608 start_motion(__goto_thread, msg->
id(),
609 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
611 msg->
phi(), msg->
theta(), msg->
psi(), __cfg_frame_kni.c_str());
620 { rot_x = msg->
rot_x(); }
622 __goto_openrave_thread->set_target(msg->
object(), rot_x);
623 #ifdef EARLY_PLANNING 624 __goto_openrave_thread->plan_target();
626 start_motion(__goto_openrave_thread, msg->
id(),
627 "Linear movement to object (%s, %f)", msg->
object(), msg->
rot_x());
633 if(__cfg_OR_enabled) {
637 __cfg_park_y * __cfg_distance_scale,
638 __cfg_park_z * __cfg_distance_scale),
641 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
642 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
643 #ifdef EARLY_PLANNING 644 __goto_openrave_thread->plan_target();
646 start_motion(__goto_openrave_thread, msg->
id(),
"Parking arm");
649 __goto_thread->
set_target(__cfg_park_x, __cfg_park_y, __cfg_park_z,
650 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
651 start_motion(__goto_thread, msg->
id(),
"Parking arm");
657 start_motion(__gripper_thread, msg->
id(),
"Opening gripper");
662 start_motion(__gripper_thread, msg->
id(),
"Closing gripper");
671 update_position(
true);
672 update_motors(
true);
675 __goto_openrave_thread->update_openrave_data();
690 if ( max_vel == 0 ) max_vel = __cfg_defmax_speed;
702 if( __cfg_OR_enabled ) {
704 __goto_openrave_thread->set_plannerparams(msg->
plannerparams());
712 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
718 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
724 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
730 start_motion(__motor_control_thread, msg->
id(),
"Moving motor");
739 __katana_if->
write();
741 #ifdef USE_TIMETRACKER 742 if (++__tt_count > 100) {
744 __tt->print_to_stdout();
virtual void init()=0
Initialize controller.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_sensor_value(unsigned int index, const uint8_t new_sensor_value)
Set sensor_value value at given index.
float rot_x() const
Get rot_x value.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
void set_enabled(bool enabled)
Set whether data acquisition is enabled or not.
virtual double y()=0
Get y-coordinate of latest endeffector position.
virtual void loop()
Code to execute in the thread.
uint32_t nr() const
Get nr 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.
void set_z(const float new_z)
Set z value.
uint8_t max_velocity() const
Get max_velocity value.
float x() const
Get x value.
LinearGotoKniMessage Fawkes BlackBoard Interface Message.
float psi() const
Get psi value.
float theta_error() const
Get theta_error 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.
float z() const
Get z 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.
float phi() const
Get phi value.
bool is_enabled() const
Get enabled value.
void set_final(const bool new_final)
Set final value.
SetMotorEncoderMessage Fawkes BlackBoard Interface Message.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
A class for handling time.
float y() const
Get y value.
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm...
virtual const char * what() const
Get primary string.
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.
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.
void set_theta(const float new_theta)
Set theta value.
virtual void read_coordinates(bool refresh=false)=0
Store current coordinates of endeeffctor.
uint32_t enc() const
Get enc value.
virtual void turn_on()=0
Turn on arm/motors.
uint32_t nr() const
Get nr value.
char * object() const
Get object value.
uint32_t enc() const
Get enc 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.
void reset()
Reset pointer.
Thread aspect to use blocked timing.
float * angles() const
Get angles value.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
float angle() const
Get angle value.
void msgq_pop()
Erase first message from queue.
bool is_enabled() const
Get enabled value.
bool finished() const
Did the motion finish already?
virtual void init()
Initialize the thread.
uint32_t nr() const
Get nr value.
OpenGripperMessage Fawkes BlackBoard Interface Message.
uint32_t nr() const
Get nr value.
virtual void reset()
Reset for next execution.
void wakeup()
Wake up thread.
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.
float psi() const
Get psi value.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void finalize()
Finalize the thread.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
unsigned int error_code() const
Error code.
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.
const char * name() const
Get name of thread.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)=0
Get sensor values.
Katana sensor acquisition thread.
void set_angles(unsigned int index, const float new_angles)
Set angles value at given index.
char * rot_frame() const
Get rot_frame value.
float angle() const
Get angle value.
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 y() const
Get y 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.
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.
size_t maxlenof_sensor_value() const
Get maximum length of sensor_value value.
float offset_xy() const
Get offset_xy value.
MoveMotorAngleMessage Fawkes BlackBoard Interface Message.
char * plannerparams() const
Get plannerparams 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.
Katana linear goto thread.
float theta() const
Get theta value.
KatanaActThread()
Constructor.
void msgq_flush()
Flush all messages.
float x() const
Get x value.
void join()
Join the thread.
MoveMotorEncoderMessage Fawkes BlackBoard Interface Message.
virtual void set_angle(unsigned int nr, float value, bool inc=false)
Set target angle value.
JointInterface Fawkes BlackBoard Interface.
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.
float phi() const
Get phi value.
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.
char * trans_frame() const
Get trans_frame 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.
FlushMessage Fawkes BlackBoard Interface Message.
float theta() const
Get theta value.
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.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void stop()=0
Stop movement immediately.
float z() const
Get z value.
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.
bool is_straight() const
Get straight value.
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.
virtual void close(Interface *interface)=0
Close interface.