Fawkes API
Fawkes Development Version
|
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()) {
void lock()
Lock this mutex.
virtual void stop()=0
Stop the current movement.
@ TRAJEC_PLANNING_ERROR
planner could not plan a collision-free trajectory.
virtual bool final()
Check if arm is final.
float finger3() const
Get finger3 value.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual bool final()=0
Check if movement is final.
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...
float y() const
Get y value.
virtual void stop()
Stops the current movement.
RefPtr<> is a reference-counting shared smartpointer.
float z() const
Get z value.
jaco_target_type_t type
target type.
float euler3() const
Get euler3 value.
@ TRAJEC_EXECUTING
trajectory is being executed.
virtual void init()
Initialize.
float finger1() const
Get finger1 value.
float x() const
Get x value.
const char * name() const
float * joints() const
Get joints value.
virtual void pos_ready()
Moves the arm to the "READY" position.
void unlock()
Unlock the mutex.
@ TARGET_GRIPPER
only gripper movement.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
@ TARGET_READY
target is the READY position of the Jaco arm.
@ TARGET_ANGULAR
target with angular coordinates.
jaco_trajec_point_t pos
target position (interpreted depending on target type).
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
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 goto_ready()=0
Move the arm to READY position.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
@ TRAJEC_SKIP
skip trajectory planning for this target.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
Jaco target struct, holding information on a target.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
@ TARGET_CARTESIAN
target with cartesian coordinates.
virtual void log_warn(const char *component, const char *format,...)=0
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)=0
Move the arm to given configuration.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float euler2() const
Get euler2 value.
virtual void finalize()
Finalize.
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
@ TARGET_RETRACT
target is the RETRACT position of the Jaco arm.
virtual const char * what() const
Get primary string.
virtual void goto_trajec(std::vector< std::vector< float >> *trajec, std::vector< float > &fingers)=0
Move the arm along the given trajectory.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
JacoGotoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
virtual ~JacoGotoThread()
Destructor.
Jaco struct containing all components required for one arm.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
bool coord
this target needs to be coordinated with targets of other arms.
@ TRAJEC_READY
trajectory has been planned and is ready for execution.
virtual void log_debug(const char *component, const char *format,...)=0
virtual void goto_retract()=0
Move the arm to RETRACT position.
float euler1() const
Get euler1 value.
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
float finger2() const
Get finger2 value.
jaco_trajec_point_t fingers
target finger values.
virtual void loop()
The main loop of this thread.