Fawkes API
Fawkes Development Version
|
Jaco Arm thread for single-arm setup, integrating OpenRAVE. More...
#include "openrave_thread.h"
Public Member Functions | |
JacoOpenraveThread (const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true) | |
Constructor. More... | |
virtual void | loop () |
Mani loop. More... | |
virtual void | finalize () |
Finalize the thread. More... | |
virtual void | update_openrave () |
Update the openrave environment to represent the current situation. More... | |
virtual void | plot_first () |
Plot the first target of the queue in the viewer_env. More... | |
virtual bool | add_target (float x, float y, float z, float e1, float e2, float e3, bool plan=true) |
Solve IK and add target to the queue. More... | |
virtual bool | add_target_ang (float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true) |
Add target joint values to the queue. More... | |
virtual bool | set_target (float x, float y, float z, float e1, float e2, float e3, bool plan=true) |
Flush the target_queue and add this one. More... | |
virtual bool | set_target_ang (float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true) |
Flush the target_queue and add this one. More... | |
![]() | |
JacoOpenraveBaseThread (const char *name) | |
Constructor. More... | |
virtual | ~JacoOpenraveBaseThread () |
Destructor. More... | |
void | init () |
Initializer. More... | |
virtual void | set_plannerparams (const std::string ¶ms) |
Set planner parameters. More... | |
virtual void | set_plannerparams (const char *params) |
Set planner parameters. More... | |
virtual void | plot_current (bool enable) |
Enable/Disable plotting of the current arm position. More... | |
![]() | |
virtual | ~Thread () |
Virtual destructor. More... | |
bool | prepare_finalize () |
Prepare finalization. More... | |
virtual bool | prepare_finalize_user () |
Prepare finalization user implementation. More... | |
void | cancel_finalize () |
Cancel finalization. More... | |
void | start (bool wait=true) |
Call this method to start the thread. More... | |
void | cancel () |
Cancel a thread. More... | |
void | join () |
Join the thread. More... | |
void | detach () |
Detach the thread. More... | |
void | kill (int sig) |
Send signal to a thread. More... | |
bool | operator== (const Thread &thread) |
Check if two threads are the same. More... | |
void | wakeup () |
Wake up thread. More... | |
void | wakeup (Barrier *barrier) |
Wake up thread and wait for barrier afterwards. More... | |
void | wait_loop_done () |
Wait for the current loop iteration to finish. More... | |
OpMode | opmode () const |
Get operation mode. More... | |
pthread_t | thread_id () const |
Get ID of thread. More... | |
bool | started () const |
Check if thread has been started. More... | |
bool | cancelled () const |
Check if thread has been cancelled. More... | |
bool | detached () const |
Check if thread has been detached. More... | |
bool | running () const |
Check if the thread is running. More... | |
bool | waiting () const |
Check if thread is currently waiting for wakeup. More... | |
const char * | name () const |
Get name of thread. More... | |
void | set_flags (uint32_t flags) |
Set all flags in one go. More... | |
void | set_flag (uint32_t flag) |
Set flag for the thread. More... | |
void | unset_flag (uint32_t flag) |
Unset flag. More... | |
bool | flagged_bad () const |
Check if FLAG_BAD was set. More... | |
void | set_delete_on_exit (bool del) |
Set whether the thread should be deleted on exit. More... | |
void | set_prepfin_hold (bool hold) |
Hold prepare_finalize(). More... | |
void | add_notification_listener (ThreadNotificationListener *notification_listener) |
Add notification listener. More... | |
void | remove_notification_listener (ThreadNotificationListener *notification_listener) |
Remove notification listener. More... | |
void | notify_of_failed_init () |
Notify of failed init. More... | |
![]() | |
LoggingAspect () | |
Constructor. More... | |
virtual | ~LoggingAspect () |
Virtual empty Destructor. More... | |
void | init_LoggingAspect (Logger *logger) |
Set the logger. More... | |
![]() | |
const std::list< const char * > & | get_aspects () const |
Get list of aspect names attached to a aspected thread. More... | |
![]() | |
ConfigurableAspect () | |
Constructor. More... | |
virtual | ~ConfigurableAspect () |
Virtual empty Destructor. More... | |
void | init_ConfigurableAspect (Configuration *config) |
Set the configuration It is guaranteed that this is called for a configurable thread before Thread::start() is called (when running regularly inside Fawkes). More... | |
![]() | |
BlackBoardAspect (const char *owner=NULL) | |
Constructor. More... | |
virtual | ~BlackBoardAspect () |
Virtual empty destructor. More... | |
void | init_BlackBoardAspect (BlackBoard *bb) |
Init BlackBoard aspect. More... | |
Protected Member Functions | |
virtual void | run () |
Stub to see name in backtrace for easier debugging. More... | |
![]() | |
Thread (const char *name) | |
Constructor. More... | |
Thread (const char *name, OpMode op_mode) | |
Constructor. More... | |
void | exit () |
Exit the thread. More... | |
void | test_cancel () |
Set cancellation point. More... | |
void | yield () |
Yield the processor to another thread or process. More... | |
void | set_opmode (OpMode op_mode) |
Set operation mode. More... | |
void | set_prepfin_conc_loop (bool concurrent=true) |
Set concurrent execution of prepare_finalize() and loop(). More... | |
void | set_coalesce_wakeups (bool coalesce=true) |
Set wakeup coalescing. More... | |
void | set_name (const char *format,...) |
Set name of thread. More... | |
virtual void | once () |
Execute an action exactly once. More... | |
bool | wakeup_pending () |
Check if wakeups are pending. More... | |
![]() | |
void | add_aspect (const char *name) |
Add an aspect to a thread. More... | |
Additional Inherited Members | |
![]() | |
enum | OpMode { OPMODE_CONTINUOUS, OPMODE_WAITFORWAKEUP } |
Thread operation mode. More... | |
enum | CancelState { CANCEL_ENABLED, CANCEL_DISABLED } |
Cancel state. More... | |
![]() | |
static Thread * | current_thread () |
Get the Thread instance of the currently running thread. More... | |
static Thread * | current_thread_noexc () throw () |
Similar to current_thread, but does never throw an exception. More... | |
static pthread_t | current_thread_id () |
Get the ID of the currently running thread. More... | |
static void | init_main () |
Initialize Thread wrapper instance for main thread. More... | |
static void | destroy_main () |
Destroy main thread wrapper instance. More... | |
static void | set_cancel_state (CancelState new_state, CancelState *old_state=0) |
Set the cancel state of the current thread. More... | |
![]() | |
static const unsigned int | FLAG_BAD = 0x00000001 |
Standard thread flag: "thread is bad". More... | |
![]() | |
fawkes::Mutex * | __planning_mutex |
mutex, used to lock when planning. More... | |
![]() | |
bool | finalize_prepared |
True if prepare_finalize() has been called and was not stopped with a cancel_finalize(), false otherwise. More... | |
Mutex * | loop_mutex |
Mutex that is used to protect a call to loop(). More... | |
Mutex * | loopinterrupt_antistarve_mutex |
Mutex to avoid starvation when trying to lock loop_mutex. More... | |
![]() | |
Logger * | logger |
This is the Logger member used to access the logger. More... | |
![]() | |
Configuration * | config |
This is the Configuration member used to access the configuration. More... | |
![]() | |
BlackBoard * | blackboard |
This is the BlackBoard instance you can use to interact with the BlackBoard. More... | |
Jaco Arm thread for single-arm setup, integrating OpenRAVE.
Definition at line 36 of file openrave_thread.h.
JacoOpenraveThread::JacoOpenraveThread | ( | const char * | name, |
fawkes::jaco_arm_t * | arm, | ||
bool | load_robot = true |
||
) |
Constructor.
name | thread name |
arm | pointer to jaco_arm_t struct, to be used in this thread |
load_robot | decide if this thread should load a robot. This should only be set to "true" if a separate OpenRaveRobot should be loaded (e.g. not the case when using 1 robot with 2 manipulators!) |
Definition at line 61 of file openrave_thread.cpp.
References fawkes::ConfigurableAspect::config, fawkes::jaco_arm_struct::config, fawkes::CONFIG_LEFT, fawkes::CONFIG_RIGHT, fawkes::CONFIG_SINGLE, finalize(), fawkes::Configuration::get_string(), fawkes::Logger::log_debug(), fawkes::LoggingAspect::logger, fawkes::Thread::name(), and fawkes::Exception::what_no_backtrace().
|
virtual |
Solve IK and add target to the queue.
The IK is solved, ignoring collisions of the end-effector with the environment. We do this to generally decide if IK is generally solvable. Collision checking is done in a later step in JacoOpenraveThread::_plan_path .
If IK is solvable, the target is enqueued in the target_queue.
x | x-coordinate of target position |
y | y-coordinate of target position |
z | z-coordinate of target position |
e1 | 1st euler rotation of target orientation |
e2 | 2nd euler rotation of target orientation |
e3 | 3rd euler rotation of target orientation |
plan | decide if we want to plan a trajectory for this or not |
Definition at line 375 of file openrave_thread.cpp.
References fawkes::jaco_target_struct_t::coord, fawkes::EULER_ZXZ, fawkes::Logger::log_debug(), fawkes::Logger::log_warn(), fawkes::LoggingAspect::logger, fawkes::Thread::name(), fawkes::jaco_target_struct_t::pos, fawkes::TARGET_ANGULAR, fawkes::TARGET_CARTESIAN, fawkes::jaco_arm_struct::target_mutex, fawkes::jaco_arm_struct::target_queue, fawkes::TRAJEC_SKIP, fawkes::jaco_target_struct_t::trajec_state, fawkes::TRAJEC_WAITING, and fawkes::jaco_target_struct_t::type.
Referenced by JacoActThread::loop(), and set_target().
|
virtual |
Add target joint values to the queue.
Use this method with caution, as for now there are no checks for validity of the target joint values. This will be added soon. Collision checking with the environment is done in a later step in JacoOpenraveThread::_plan_path .
j1 | target angle of 1st joint |
j2 | target angle of 2nd joint |
j3 | target angle of 3rd joint |
j4 | target angle of 4th joint |
j5 | target angle of 5th joint |
j6 | target angle of 6th joint |
plan | decide if we want to plan a trajectory for this or not |
Definition at line 467 of file openrave_thread.cpp.
References fawkes::jaco_target_struct_t::coord, fawkes::jaco_target_struct_t::pos, fawkes::TARGET_ANGULAR, fawkes::jaco_arm_struct::target_mutex, fawkes::jaco_arm_struct::target_queue, fawkes::TRAJEC_SKIP, fawkes::jaco_target_struct_t::trajec_state, fawkes::TRAJEC_WAITING, and fawkes::jaco_target_struct_t::type.
Referenced by JacoActThread::loop(), and set_target_ang().
|
virtual |
Finalize the thread.
This method is executed just before the thread is canceled and destroyed. It is always preceeded by a call to prepare_finalize(). If this is not the case this is a failure. The condition can be checked with the boolean variable finalize_prepared.
This method is meant to be used in conjunction with aspects and to cover thread inter-dependencies. This routine MUST bring the thread into a safe state such that it may be canceled and destroyed afterwards. If there is any reason that this cannot happen make your prepare_finalize() reports so.
This method is called by the thread manager just before the thread is being cancelled. Here you can do whatever steps are necessary just before the thread is cancelled. Note that you thread is still running and might be in the middle of a loop, so it is not a good place to give up on all resources used. Mind segmentation faults that could happen. Protect the area with a mutex that you lock at the beginning of your loop and free in the end, and that you lock at the beginning of finalize and then never unlock. Also not that the finalization may be canceled afterwards. The next thing that happens is that either the thread is canceled and destroyed or that the finalization is canceled and the thread has to run again.
Finalize is called on a thread just before it is deleted. It is guaranteed to be called on a fully initialized thread (if no exception is thrown in init()) (this guarantee holds in the Fawkes framework).
The default implementation does nothing besides throwing an exception if prepare_finalize() has not been called.
Exception | thrown if prepare_finalize() has not been called. |
Reimplemented from JacoOpenraveBaseThread.
Definition at line 199 of file openrave_thread.cpp.
References JacoOpenraveBaseThread::finalize().
Referenced by JacoOpenraveThread().
|
virtual |
Mani loop.
It iterates over the target_queue to find the first target that needs trajectory planning. This can be done if it is the first target, or if the previous target has a known final configuration, which can be used as the current starting configuration. The result is stored in the struct of the current target, which can then be processed by the goto_thread
Reimplemented from fawkes::Thread.
Definition at line 224 of file openrave_thread.cpp.
References JacoOpenraveBaseThread::__planning_mutex, fawkes::jaco_arm_struct::arm, fawkes::JacoArm::get_joints(), fawkes::Mutex::lock(), fawkes::jaco_target_struct_t::pos, fawkes::TARGET_ANGULAR, fawkes::TARGET_GRIPPER, fawkes::jaco_arm_struct::target_mutex, fawkes::jaco_arm_struct::target_queue, fawkes::TRAJEC_EXECUTING, fawkes::TRAJEC_READY, fawkes::TRAJEC_SKIP, fawkes::TRAJEC_WAITING, and fawkes::Mutex::unlock().
|
virtual |
Plot the first target of the queue in the viewer_env.
Implements JacoOpenraveBaseThread.
Definition at line 689 of file openrave_thread.cpp.
References fawkes::OpenRaveManipulator::copy(), fawkes::OpenRaveManipulator::get_angles(), fawkes::OpenRaveManipulator::set_angles_device(), fawkes::jaco_arm_struct::target_mutex, fawkes::jaco_arm_struct::target_queue, fawkes::jaco_target_struct_t::trajec, fawkes::jaco_arm_struct::trajec_color, fawkes::TRAJEC_EXECUTING, fawkes::jaco_arm_struct::trajec_mutex, fawkes::TRAJEC_READY, and fawkes::jaco_target_struct_t::trajec_state.
Referenced by JacoGotoThread::loop().
|
inlineprotectedvirtual |
Stub to see name in backtrace for easier debugging.
Reimplemented from JacoOpenraveBaseThread.
Definition at line 54 of file openrave_thread.h.
|
virtual |
Flush the target_queue and add this one.
see JacoOpenraveThread::add_target for that.
x | x-coordinate of target position |
y | y-coordinate of target position |
z | z-coordinate of target position |
e1 | 1st euler rotation of target orientation |
e2 | 2nd euler rotation of target orientation |
e3 | 3rd euler rotation of target orientation |
plan | decide if we want to plan a trajectory for this or not |
Definition at line 518 of file openrave_thread.cpp.
References add_target(), fawkes::RefPtr< T_CppObject >::clear(), fawkes::jaco_arm_struct::target_mutex, and fawkes::jaco_arm_struct::target_queue.
|
virtual |
Flush the target_queue and add this one.
see JacoOpenraveThread::add_target_ang for that.
j1 | target angle of 1st joint |
j2 | target angle of 2nd joint |
j3 | target angle of 3rd joint |
j4 | target angle of 4th joint |
j5 | target angle of 5th joint |
j6 | target angle of 6th joint |
plan | decide if we want to plan a trajectory for this or not |
Definition at line 540 of file openrave_thread.cpp.
References add_target_ang(), fawkes::RefPtr< T_CppObject >::clear(), fawkes::EULER_ZXZ, fawkes::Logger::log_debug(), fawkes::Logger::log_warn(), fawkes::LoggingAspect::logger, fawkes::Thread::name(), fawkes::jaco_target_struct_t::pos, fawkes::TARGET_ANGULAR, fawkes::TARGET_CARTESIAN, fawkes::jaco_arm_struct::target_mutex, fawkes::jaco_arm_struct::target_queue, fawkes::jaco_target_struct_t::trajec, fawkes::jaco_arm_struct::trajec_mutex, fawkes::TRAJEC_PLANNING, fawkes::TRAJEC_PLANNING_ERROR, fawkes::TRAJEC_READY, fawkes::jaco_target_struct_t::trajec_state, fawkes::jaco_target_struct_t::type, and fawkes::Exception::what_no_backtrace().
|
virtual |
Update the openrave environment to represent the current situation.
This includes updating the model and plotting current positions.
Implements JacoOpenraveBaseThread.
Definition at line 290 of file openrave_thread.cpp.
References fawkes::deg2rad(), fawkes::JacoInterface::finger1(), fawkes::JacoInterface::finger2(), fawkes::JacoInterface::finger3(), fawkes::jaco_arm_struct::iface, and fawkes::JacoInterface::joints().
Referenced by JacoActThread::loop().