23 #include "goto_thread.h"
26 #include "openrave_thread.h"
28 #include <core/threading/mutex.h>
29 #include <interfaces/JacoInterface.h>
30 #include <utils/math/angle.h>
55 wait_status_check_ = 0;
67 final_mutex_ =
new Mutex();
143 target->coord =
false;
145 target->pos.push_back(x);
146 target->pos.push_back(y);
147 target->pos.push_back(z);
148 target->pos.push_back(e1);
149 target->pos.push_back(e2);
150 target->pos.push_back(e3);
152 if (f1 > 0.f && f2 > 0.f && f3 > 0.f) {
153 target->fingers.push_back(f1);
154 target->fingers.push_back(f2);
155 target->fingers.push_back(f3);
190 target->coord =
false;
192 target->pos.push_back(j1);
193 target->pos.push_back(j2);
194 target->pos.push_back(j3);
195 target->pos.push_back(j4);
196 target->pos.push_back(j5);
197 target->pos.push_back(j6);
199 if (f1 > 0.f && f2 > 0.f && f3 > 0.f) {
200 target->fingers.push_back(f1);
201 target->fingers.push_back(f2);
202 target->fingers.push_back(f3);
248 target->fingers.push_back(f1);
249 target->fingers.push_back(f2);
250 target->fingers.push_back(f3);
272 final_mutex_->
lock();
290 final_mutex_->
lock();
294 if (arm_ == NULL || arm_->
arm == NULL || !
final) {
317 if (!target_ || target_->coord) {
325 switch (target_->trajec_state) {
329 name(),
"No planning for this new target. Process, using current finger positions...");
350 if (!target_->trajec->empty()) {
358 _exec_trajec(*(target_->trajec));
379 JacoGotoThread::_check_final()
381 bool check_fingers =
false;
385 switch (target_->type) {
388 if (wait_status_check_ == 0) {
390 final_mutex_->
lock();
393 }
else if (wait_status_check_ >= 10) {
394 wait_status_check_ = 0;
396 ++wait_status_check_;
403 for (
unsigned int i = 0; i < 6; ++i) {
407 final_mutex_->
lock();
410 check_fingers =
true;
415 final_mutex_->
lock();
418 check_fingers =
true;
430 final_mutex_->
lock();
434 check_fingers =
true;
438 final_mutex_->
lock();
443 if (check_fingers &&
final) {
449 finger_last_[3] += 1;
456 final_mutex_->
lock();
457 final_ &= finger_last_[3] > 10;
463 JacoGotoThread::_goto_target()
470 final_mutex_->
lock();
479 if (target_->type == TARGET_GRIPPER) {
484 target_->pos.clear();
494 switch (target_->type) {
497 if (target_->fingers.empty()) {
508 wait_status_check_ = 0;
514 wait_status_check_ = 0;
520 if (target_->fingers.empty()) {
538 final_mutex_->
lock();
542 if (target_->fingers.empty()) {
virtual void loop()
The main loop of this thread.
virtual bool final()
Check if arm is final.
virtual void finalize()
Finalize.
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
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...
JacoGotoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
virtual void init()
Initialize.
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.
virtual void stop()
Stops the current movement.
virtual void pos_ready()
Moves the arm to the "READY" position.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual ~JacoGotoThread()
Destructor.
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
Base class for exceptions in Fawkes.
virtual const char * what() const
Get primary string.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
virtual void stop()=0
Stop the current movement.
virtual void goto_retract()=0
Move the arm to RETRACT position.
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)=0
Move the arm to given configuration.
virtual void goto_trajec(std::vector< std::vector< float >> *trajec, std::vector< float > &fingers)=0
Move the arm along the given trajectory.
virtual bool final()=0
Check if movement is final.
virtual void goto_ready()=0
Move the arm to READY position.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
float finger3() const
Get finger3 value.
float finger2() const
Get finger2 value.
float x() const
Get x value.
float euler3() const
Get euler3 value.
float finger1() const
Get finger1 value.
float * joints() const
Get joints value.
float euler2() const
Get euler2 value.
float z() const
Get z value.
float euler1() const
Get euler1 value.
float y() const
Get y value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Mutex mutual exclusion lock.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
RefPtr<> is a reference-counting shared smartpointer.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
@ TARGET_READY
target is the READY position of the Jaco arm.
@ TARGET_GRIPPER
only gripper movement.
@ TARGET_CARTESIAN
target with cartesian coordinates.
@ TARGET_RETRACT
target is the RETRACT position of the Jaco arm.
@ TARGET_ANGULAR
target with angular coordinates.
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
@ TRAJEC_READY
trajectory has been planned and is ready for execution.
@ TRAJEC_PLANNING_ERROR
planner could not plan a collision-free trajectory.
@ TRAJEC_EXECUTING
trajectory is being executed.
@ TRAJEC_SKIP
skip trajectory planning for this target.
Jaco struct containing all components required for one arm.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Jaco target struct, holding information on a target.