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