23 #include "goto_thread.h" 24 #include "openrave_thread.h" 27 #include <interfaces/JacoInterface.h> 28 #include <utils/math/angle.h> 29 #include <core/threading/mutex.h> 54 __wait_status_check = 0;
67 __final_mutex =
new Mutex();
90 __final_mutex->
lock();
96 __final_mutex->
lock();
131 float e1,
float e2,
float e3,
132 float f1,
float f2,
float f3)
137 target->
coord =
false;
139 target->
pos.push_back(x);
140 target->
pos.push_back(y);
141 target->
pos.push_back(z);
142 target->
pos.push_back(e1);
143 target->
pos.push_back(e2);
144 target->
pos.push_back(e3);
146 if( f1 > 0.f && f2 > 0.f && f3 > 0.f ) {
172 float j4,
float j5,
float j6,
173 float f1,
float f2,
float f3)
178 target->
coord =
false;
180 target->
pos.push_back(j1);
181 target->
pos.push_back(j2);
182 target->
pos.push_back(j3);
183 target->
pos.push_back(j4);
184 target->
pos.push_back(j5);
185 target->
pos.push_back(j6);
187 if( f1 > 0.f && f2 > 0.f && f3 > 0.f ) {
260 __final_mutex->
lock();
278 __final_mutex->
lock();
279 bool final = __final;
282 if(__arm == NULL || __arm->
arm == NULL || !
final) {
305 if( !__target || __target->coord ) {
313 switch( __target->trajec_state ) {
316 logger->
log_debug(
name(),
"No planning for this new target. Process, using current finger positions...");
337 if( !__target->trajec->empty() ) {
345 _exec_trajec(*(__target->trajec));
368 JacoGotoThread::_check_final()
370 bool check_fingers =
false;
374 switch( __target->type ) {
378 if( __wait_status_check == 0 ) {
380 __final_mutex->
lock();
383 }
else if( __wait_status_check >= 10 ) {
384 __wait_status_check = 0;
386 ++__wait_status_check;
393 for(
unsigned int i=0; i<6; ++i ) {
396 __final_mutex->
lock();
399 check_fingers =
true;
404 __final_mutex->
lock();
407 check_fingers =
true;
419 __final_mutex->
lock();
423 check_fingers =
true;
428 __final_mutex->
lock();
433 if( check_fingers &&
final ) {
440 __finger_last[3] += 1;
445 __finger_last[3] = 0;
447 __final_mutex->
lock();
448 __final &= __finger_last[3] > 10;
454 JacoGotoThread::_goto_target()
459 __finger_last[3] = 0;
461 __final_mutex->
lock();
475 __target->pos.clear();
485 switch( __target->type ) {
488 if( __target->fingers.empty() ) {
499 __wait_status_check = 0;
505 __wait_status_check = 0;
511 if( __target->fingers.empty() ) {
529 __final_mutex->
lock();
533 if( __target->fingers.empty() ) {
trajectory has been planned and is ready for execution.
float z() const
Get z value.
Jaco struct containing all components required for one arm.
JacoGotoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Fawkes library namespace.
void unlock()
Unlock the mutex.
virtual void goto_retract()=0
Move the arm to RETRACT position.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
virtual void init()
Initialize.
float finger2() const
Get finger2 value.
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual const char * what() const
Get primary string.
Thread class encapsulation of pthreads.
virtual void set_target(float x, float y, float z, float e1, float e2, float e3, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given cartesian coordinates.
trajectory is being executed.
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Logger * logger
This is the Logger member used to access the logger.
jaco_target_type_t type
target type.
planner could not plan a collision-free trajectory.
jaco_trajec_point_t pos
target position (interpreted depending on target type).
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
float euler3() const
Get euler3 value.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
virtual ~JacoGotoThread()
Destructor.
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
virtual void pos_ready()
Moves the arm to the "READY" position.
float euler2() const
Get euler2 value.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
float finger1() const
Get finger1 value.
virtual void stop()
Stops the current movement.
float x() const
Get x value.
float * joints() const
Get joints value.
Base class for exceptions in Fawkes.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
target with cartesian coordinates.
virtual void finalize()
Finalize.
virtual bool final()
Check if arm is final.
Jaco target struct, holding information on a target.
target with angular coordinates.
virtual void goto_trajec(std::vector< std::vector< float > > *trajec, std::vector< float > &fingers)=0
Move the arm along the given trajectory.
const char * name() const
Get name of thread.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void goto_ready()=0
Move the arm to READY position.
float euler1() const
Get euler1 value.
bool coord
this target needs to be coordinated with targets of other arms.
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
RefPtr<> is a reference-counting shared smartpointer.
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)=0
Move the arm to given configuration.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
skip trajectory planning for this target.
virtual void loop()
The main loop of this thread.
virtual bool final()=0
Check if movement is final.
void lock()
Lock this mutex.
target is the RETRACT position of the Jaco arm.
virtual void stop()=0
Stop the current movement.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float finger3() const
Get finger3 value.
target is the READY position of the Jaco arm.
jaco_trajec_point_t fingers
target finger values.
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.
float y() const
Get y value.
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
virtual void set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given joint positions This target is added to the queue, skipping trajectory planning...