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();
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual void stop()
Stops the current movement.
virtual void init()
Initialize the thread.
virtual void loop()
The main loop of this thread.
virtual bool final()
Check if arm is final.
virtual ~JacoBimanualGotoThread()
Destructor.
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 finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Abstract class for a Kinova Jaco Arm that we want to control.
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
void set_error_code(const uint32_t new_error_code)
Set error_code 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.
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_GRIPPER
only gripper movement.
@ TARGET_ANGULAR
target with angular coordinates.
std::vector< float > jaco_trajec_point_t
A trajectory point.
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
@ TRAJEC_READY
trajectory has been planned and is ready for execution.
@ TRAJEC_IK_ERROR
planner could not find IK solution for target
@ 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 a dual-arm setup.
JacoBimanualInterface * iface
interface used for coordinated manipulation.
jaco_arm_t * right
the struct with all the data for the right arm.
jaco_arm_t * left
the struct with all the data for the left arm.
Jaco target struct, holding information on a target.