23 #include "bimanual_goto_thread.h" 26 #include "goto_thread.h" 27 #include "openrave_thread.h" 29 #include <core/threading/mutex.h> 30 #include <interfaces/JacoBimanualInterface.h> 31 #include <interfaces/JacoInterface.h> 32 #include <utils/math/angle.h> 49 :
Thread(
"JacoBimanualGotoThread",
Thread::OPMODE_CONTINUOUS)
64 arms_.l.arm = dual_arms_->
left;
65 arms_.r.arm = dual_arms_->
right;
67 final_mutex_ =
new Mutex();
68 v_arms_[0] = &(arms_.l);
69 v_arms_[1] = &(arms_.r);
97 if (arms_.l.arm == NULL || arms_.r.arm == NULL || !
final) {
103 if (arms_.l.target && arms_.r.target) {
104 arms_.l.target.clear();
105 arms_.r.target.clear();
110 arms_.l.arm->target_queue->pop_front();
111 arms_.r.arm->target_queue->pop_front();
117 if (!arms_.l.arm->target_queue->empty() && !arms_.r.arm->target_queue->empty()) {
119 arms_.l.target = arms_.l.arm->target_queue->front();
120 arms_.r.target = arms_.r.arm->target_queue->front();
124 if (!arms_.l.target || !arms_.r.target || !arms_.l.target->coord || !arms_.r.target->coord) {
127 arms_.l.target.clear();
128 arms_.r.target.clear();
133 if (arms_.l.target->type != arms_.r.target->type) {
135 "target type mismatch, %i != %i",
136 arms_.l.target->type,
137 arms_.r.target->type);
138 arms_.l.target.clear();
139 arms_.r.target.clear();
155 if (arms_.l.target->trajec_state != arms_.r.target->trajec_state) {
157 "trajec state mismatch, %i != %i",
158 arms_.l.target->trajec_state,
159 arms_.r.target->trajec_state);
160 arms_.l.target.clear();
161 arms_.r.target.clear();
166 switch (arms_.l.target->trajec_state) {
171 "No planning for these targets. Process, using current finger positions...");
175 "Unknown target type %i, cannot process without planning!",
176 arms_.l.target->type);
194 if (!arms_.l.target->trajec->empty() && !arms_.r.target->trajec->empty()) {
196 arms_.l.arm->openrave_thread->plot_first();
197 arms_.r.arm->openrave_thread->plot_first();
200 arms_.l.arm->openrave_thread->plot_current(
true);
201 arms_.r.arm->openrave_thread->plot_current(
true);
211 arms_.l.target.clear();
212 arms_.r.target.clear();
226 final_mutex_->
lock();
232 final_mutex_->
lock();
242 final = arms_.l.arm->target_queue->empty() && arms_.r.arm->target_queue->empty();
254 arms_.l.arm->goto_thread->stop();
255 arms_.r.arm->goto_thread->stop();
257 arms_.l.target.clear();
258 arms_.r.target.clear();
260 final_mutex_->
lock();
287 target_l->
coord =
true;
288 target_r->
coord =
true;
290 target_l->
fingers.push_back(l_f1);
291 target_l->
fingers.push_back(l_f2);
292 target_l->
fingers.push_back(l_f3);
293 target_r->
fingers.push_back(l_f1);
294 target_r->
fingers.push_back(l_f2);
295 target_r->
fingers.push_back(l_f3);
298 _enqueue_targets(target_l, target_r);
304 JacoBimanualGotoThread::_lock_queues()
const 306 arms_.l.arm->target_mutex->lock();
307 arms_.r.arm->target_mutex->lock();
311 JacoBimanualGotoThread::_unlock_queues()
const 313 arms_.l.arm->target_mutex->unlock();
314 arms_.r.arm->target_mutex->unlock();
320 arms_.l.arm->target_queue->push_back(l);
321 arms_.r.arm->target_queue->push_back(r);
325 JacoBimanualGotoThread::_move_grippers()
327 final_mutex_->
lock();
331 for (
unsigned int i = 0; i < 2; ++i) {
332 v_arms_[i]->finger_last[0] = v_arms_[i]->arm->iface->finger1();
333 v_arms_[i]->finger_last[1] = v_arms_[i]->arm->iface->finger2();
334 v_arms_[i]->finger_last[2] = v_arms_[i]->arm->iface->finger3();
335 v_arms_[i]->finger_last[3] = 0;
344 for (
unsigned int i = 0; i < 2; ++i) {
345 v_arms_[i]->target->pos.clear();
346 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(0));
347 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(1));
348 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(2));
349 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(3));
350 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(4));
351 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(5));
356 arms_.l.arm->arm->goto_joints(arms_.l.target->pos, arms_.l.target->fingers);
357 arms_.r.arm->arm->goto_joints(arms_.r.target->pos, arms_.r.target->fingers);
365 JacoBimanualGotoThread::_exec_trajecs()
367 final_mutex_->
lock();
371 for (
unsigned int i = 0; i < 2; ++i) {
372 if (v_arms_[i]->target->fingers.empty()) {
374 v_arms_[i]->target->fingers.push_back(v_arms_[i]->arm->iface->finger1());
375 v_arms_[i]->target->fingers.push_back(v_arms_[i]->arm->iface->finger2());
376 v_arms_[i]->target->fingers.push_back(v_arms_[i]->arm->iface->finger3());
382 arms_.l.arm->arm->stop();
383 arms_.r.arm->arm->stop();
389 unsigned int first = 0;
390 unsigned int second = 1;
391 if (v_arms_[1]->target->trajec->size() < v_arms_[0]->target->trajec->size()) {
395 JacoArm * arm_first = v_arms_[first]->arm->arm;
396 JacoArm * arm_second = v_arms_[second]->arm->arm;
397 jaco_trajec_t *trajec_first = *(v_arms_[first]->target->trajec);
398 jaco_trajec_t *trajec_second = *(v_arms_[second]->target->trajec);
399 unsigned int size_first = trajec_first->size();
400 unsigned int size_second = trajec_second->size();
405 for (
unsigned int i = 0; i < 2; ++i) {
407 cur.push_back(v_arms_[i]->arm->iface->joints(0));
408 cur.push_back(v_arms_[i]->arm->iface->joints(1));
409 cur.push_back(v_arms_[i]->arm->iface->joints(2));
410 cur.push_back(v_arms_[i]->arm->iface->joints(3));
411 cur.push_back(v_arms_[i]->arm->iface->joints(4));
412 cur.push_back(v_arms_[i]->arm->iface->joints(5));
413 v_arms_[i]->arm->arm->goto_joints(cur, v_arms_[i]->target->fingers,
false);
419 while (it < size_first) {
421 v_arms_[first]->target->fingers,
424 v_arms_[second]->target->fingers,
430 while (it < size_second) {
432 v_arms_[second]->target->fingers,
445 JacoBimanualGotoThread::_check_final()
450 for (
unsigned int i = 0; i < 2; ++i) {
451 switch (v_arms_[i]->target->type) {
454 for (
unsigned int j = 0; j < 6; ++j) {
456 deg2rad(v_arms_[i]->arm->iface->joints(j)))
463 final &= v_arms_[i]->arm->arm->final();
477 for (
unsigned int i = 0; i < 2; ++i) {
480 if (v_arms_[i]->finger_last[0] == v_arms_[i]->arm->iface->finger1()
481 && v_arms_[i]->finger_last[1] == v_arms_[i]->arm->iface->finger2()
482 && v_arms_[i]->finger_last[2] == v_arms_[i]->arm->iface->finger3()) {
483 v_arms_[i]->finger_last[3] += 1;
485 v_arms_[i]->finger_last[0] = v_arms_[i]->arm->iface->finger1();
486 v_arms_[i]->finger_last[1] = v_arms_[i]->arm->iface->finger2();
487 v_arms_[i]->finger_last[2] = v_arms_[i]->arm->iface->finger3();
488 v_arms_[i]->finger_last[3] = 0;
490 final &= v_arms_[i]->finger_last[3] > 10;
495 final_mutex_->
lock();
trajectory has been planned and is ready for execution.
virtual void finalize()
Finalize the thread.
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Fawkes library namespace.
void unlock()
Unlock the mutex.
virtual void loop()
The main loop of this thread.
virtual ~JacoBimanualGotoThread()
Destructor.
Thread class encapsulation of pthreads.
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_arm_t * left
the struct with all the data for the left arm.
jaco_arm_t * right
the struct with all the data for the right arm.
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
std::vector< float > jaco_trajec_point_t
A trajectory point.
virtual bool final()
Check if arm is final.
Base class for exceptions in Fawkes.
Jaco target struct, holding information on a target.
target with angular coordinates.
const char * name() const
Get name of thread.
virtual void init()
Initialize the thread.
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 move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3)
Moves only the gripper of both arms.
virtual void stop()
Stops the current movement.
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.
planner could not find IK solution for target
RefPtr<> is a reference-counting shared smartpointer.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
skip trajectory planning for this target.
void lock()
Lock this mutex.
Jaco struct containing all components required for a dual-arm setup.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
jaco_trajec_point_t fingers
target finger values.
Abstract class for a Kinova Jaco Arm that we want to control.
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.