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) {
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) {
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()) {
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.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
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.
virtual void goto_trajec(std::vector< std::vector< float >> *trajec, std::vector< float > &fingers)=0
Move the arm along the given trajectory.
target with cartesian coordinates.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
virtual void finalize()
Finalize.
virtual bool final()
Check if arm is final.
Jaco target struct, holding information on a target.
target with angular coordinates.
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<> 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...