Fawkes API  Fawkes Development Version
JacoOpenraveThread Class Reference

Jaco Arm thread for single-arm setup, integrating OpenRAVE. More...

#include "openrave_thread.h"

Inheritance diagram for JacoOpenraveThread:

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...
 
- Public Member Functions inherited from JacoOpenraveBaseThread
 JacoOpenraveBaseThread (const char *name)
 Constructor. More...
 
virtual ~JacoOpenraveBaseThread ()
 Destructor. More...
 
void init ()
 Initializer. More...
 
virtual void set_plannerparams (const std::string &params)
 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...
 
- Public Member Functions inherited from fawkes::Thread
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...
 
- Public Member Functions inherited from fawkes::LoggingAspect
 LoggingAspect ()
 Constructor. More...
 
virtual ~LoggingAspect ()
 Virtual empty Destructor. More...
 
void init_LoggingAspect (Logger *logger)
 Set the logger. More...
 
- Public Member Functions inherited from fawkes::Aspect
const std::list< const char * > & get_aspects () const
 Get list of aspect names attached to a aspected thread. More...
 
- Public Member Functions inherited from fawkes::ConfigurableAspect
 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...
 
- Public Member Functions inherited from fawkes::BlackBoardAspect
 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...
 
- Protected Member Functions inherited from fawkes::Thread
 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...
 
- Protected Member Functions inherited from fawkes::Aspect
void add_aspect (const char *name)
 Add an aspect to a thread. More...
 

Additional Inherited Members

- Public Types inherited from fawkes::Thread
enum  OpMode { OPMODE_CONTINUOUS, OPMODE_WAITFORWAKEUP }
 Thread operation mode. More...
 
enum  CancelState { CANCEL_ENABLED, CANCEL_DISABLED }
 Cancel state. More...
 
- Static Public Member Functions inherited from fawkes::Thread
static Threadcurrent_thread ()
 Get the Thread instance of the currently running thread. More...
 
static Threadcurrent_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 Public Attributes inherited from fawkes::Thread
static const unsigned int FLAG_BAD = 0x00000001
 Standard thread flag: "thread is bad". More...
 
- Protected Attributes inherited from JacoOpenraveBaseThread
fawkes::Mutex__planning_mutex
 mutex, used to lock when planning. More...
 
- Protected Attributes inherited from fawkes::Thread
bool finalize_prepared
 True if prepare_finalize() has been called and was not stopped with a cancel_finalize(), false otherwise. More...
 
Mutexloop_mutex
 Mutex that is used to protect a call to loop(). More...
 
Mutexloopinterrupt_antistarve_mutex
 Mutex to avoid starvation when trying to lock loop_mutex. More...
 
- Protected Attributes inherited from fawkes::LoggingAspect
Loggerlogger
 This is the Logger member used to access the logger. More...
 
- Protected Attributes inherited from fawkes::ConfigurableAspect
Configurationconfig
 This is the Configuration member used to access the configuration. More...
 
- Protected Attributes inherited from fawkes::BlackBoardAspect
BlackBoardblackboard
 This is the BlackBoard instance you can use to interact with the BlackBoard. More...
 

Detailed Description

Jaco Arm thread for single-arm setup, integrating OpenRAVE.

Author
Bahram Maleki-Fard

Definition at line 36 of file openrave_thread.h.

Constructor & Destructor Documentation

◆ JacoOpenraveThread()

JacoOpenraveThread::JacoOpenraveThread ( const char *  name,
fawkes::jaco_arm_t arm,
bool  load_robot = true 
)

Constructor.

Parameters
namethread name
armpointer to jaco_arm_t struct, to be used in this thread
load_robotdecide 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().

Member Function Documentation

◆ add_target()

bool JacoOpenraveThread::add_target ( float  x,
float  y,
float  z,
float  e1,
float  e2,
float  e3,
bool  plan = true 
)
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.

Parameters
xx-coordinate of target position
yy-coordinate of target position
zz-coordinate of target position
e11st euler rotation of target orientation
e22nd euler rotation of target orientation
e33rd euler rotation of target orientation
plandecide if we want to plan a trajectory for this or not
Returns
"true", if IK could be solved. "false" otherwise

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

◆ add_target_ang()

bool JacoOpenraveThread::add_target_ang ( float  j1,
float  j2,
float  j3,
float  j4,
float  j5,
float  j6,
bool  plan = true 
)
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 .

Parameters
j1target angle of 1st joint
j2target angle of 2nd joint
j3target angle of 3rd joint
j4target angle of 4th joint
j5target angle of 5th joint
j6target angle of 6th joint
plandecide if we want to plan a trajectory for this or not
Returns
"true", if the target joints are valid and not in self-collision, "false" otherwise. CAUTION: Self-collision is not checked yet, this feature will be added soon.

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

◆ finalize()

void JacoOpenraveThread::finalize ( )
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.

Exceptions
Exceptionthrown if prepare_finalize() has not been called.
See also
prepare_finalize()
cancel_finalize()

Reimplemented from JacoOpenraveBaseThread.

Definition at line 199 of file openrave_thread.cpp.

References JacoOpenraveBaseThread::finalize().

Referenced by JacoOpenraveThread().

◆ loop()

void JacoOpenraveThread::loop ( )
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

See also
JacoGotoThread::loop to see how goto_thread processes the queue

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

◆ plot_first()

◆ run()

virtual void JacoOpenraveThread::run ( )
inlineprotectedvirtual

Stub to see name in backtrace for easier debugging.

See also
Thread::run()

Reimplemented from JacoOpenraveBaseThread.

Definition at line 54 of file openrave_thread.h.

◆ set_target()

bool JacoOpenraveThread::set_target ( float  x,
float  y,
float  z,
float  e1,
float  e2,
float  e3,
bool  plan = true 
)
virtual

Flush the target_queue and add this one.

see JacoOpenraveThread::add_target for that.

Parameters
xx-coordinate of target position
yy-coordinate of target position
zz-coordinate of target position
e11st euler rotation of target orientation
e22nd euler rotation of target orientation
e33rd euler rotation of target orientation
plandecide if we want to plan a trajectory for this or not
Returns
"true", if IK could be solved. "false" otherwise

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.

◆ set_target_ang()

bool JacoOpenraveThread::set_target_ang ( float  j1,
float  j2,
float  j3,
float  j4,
float  j5,
float  j6,
bool  plan = true 
)
virtual

Flush the target_queue and add this one.

see JacoOpenraveThread::add_target_ang for that.

Parameters
j1target angle of 1st joint
j2target angle of 2nd joint
j3target angle of 3rd joint
j4target angle of 4th joint
j5target angle of 5th joint
j6target angle of 6th joint
plandecide if we want to plan a trajectory for this or not
Returns
"true", if IK could be solved. "false" otherwise

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

◆ update_openrave()

void JacoOpenraveThread::update_openrave ( )
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().


The documentation for this class was generated from the following files: