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