23 #include "bimanual_goto_thread.h" 24 #include "goto_thread.h" 25 #include "openrave_thread.h" 28 #include <interfaces/JacoInterface.h> 29 #include <interfaces/JacoBimanualInterface.h> 30 #include <utils/math/angle.h> 31 #include <core/threading/mutex.h> 48 :
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);
93 __final_mutex->
lock();
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 ) {
134 logger->
log_debug(
name(),
"target type mismatch, %i != %i", __arms.l.target->type, __arms.r.target->type);
135 __arms.l.target.clear();
136 __arms.r.target.clear();
152 if( __arms.l.target->trajec_state != __arms.r.target->trajec_state ) {
153 logger->
log_debug(
name(),
"trajec state mismatch, %i != %i", __arms.l.target->trajec_state, __arms.r.target->trajec_state);
154 __arms.l.target.clear();
155 __arms.r.target.clear();
160 switch( __arms.l.target->trajec_state ) {
164 logger->
log_debug(
name(),
"No planning for these targets. Process, using current finger positions...");
167 logger->
log_warn(
name(),
"Unknown target type %i, cannot process without planning!", __arms.l.target->type);
185 if( !__arms.l.target->trajec->empty() && !__arms.r.target->trajec->empty() ) {
187 __arms.l.arm->openrave_thread->plot_first();
188 __arms.r.arm->openrave_thread->plot_first();
191 __arms.l.arm->openrave_thread->plot_current(
true);
192 __arms.r.arm->openrave_thread->plot_current(
true);
202 __arms.l.target.clear();
203 __arms.r.target.clear();
219 __final_mutex->
lock();
220 bool final = __final;
225 __final_mutex->
lock();
235 final = __arms.l.arm->target_queue->empty() && __arms.r.arm->target_queue->empty();
247 __arms.l.arm->goto_thread->stop();
248 __arms.r.arm->goto_thread->stop();
250 __arms.l.target.clear();
251 __arms.r.target.clear();
253 __final_mutex->
lock();
276 target_l->
coord=
true;
277 target_r->
coord=
true;
279 target_l->
fingers.push_back(l_f1);
280 target_l->
fingers.push_back(l_f2);
281 target_l->
fingers.push_back(l_f3);
282 target_r->
fingers.push_back(l_f1);
283 target_r->
fingers.push_back(l_f2);
284 target_r->
fingers.push_back(l_f3);
287 _enqueue_targets(target_l, target_r);
294 JacoBimanualGotoThread::_lock_queues()
const 296 __arms.l.arm->target_mutex->lock();
297 __arms.r.arm->target_mutex->lock();
301 JacoBimanualGotoThread::_unlock_queues()
const 303 __arms.l.arm->target_mutex->unlock();
304 __arms.r.arm->target_mutex->unlock();
310 __arms.l.arm->target_queue->push_back(l);
311 __arms.r.arm->target_queue->push_back(r);
315 JacoBimanualGotoThread::_move_grippers()
317 __final_mutex->
lock();
321 for(
unsigned int i=0; i<2; ++i) {
322 __v_arms[i]->finger_last[0] = __v_arms[i]->arm->iface->finger1();
323 __v_arms[i]->finger_last[1] = __v_arms[i]->arm->iface->finger2();
324 __v_arms[i]->finger_last[2] = __v_arms[i]->arm->iface->finger3();
325 __v_arms[i]->finger_last[3] = 0;
334 for(
unsigned int i=0; i<2; ++i) {
335 __v_arms[i]->target->pos.clear();
336 __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(0));
337 __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(1));
338 __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(2));
339 __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(3));
340 __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(4));
341 __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(5));
346 __arms.l.arm->arm->goto_joints(__arms.l.target->pos, __arms.l.target->fingers);
347 __arms.r.arm->arm->goto_joints(__arms.r.target->pos, __arms.r.target->fingers);
355 JacoBimanualGotoThread::_exec_trajecs()
357 __final_mutex->
lock();
361 for(
unsigned int i=0; i<2; ++i) {
362 if( __v_arms[i]->target->fingers.empty() ) {
364 __v_arms[i]->target->fingers.push_back(__v_arms[i]->arm->iface->finger1());
365 __v_arms[i]->target->fingers.push_back(__v_arms[i]->arm->iface->finger2());
366 __v_arms[i]->target->fingers.push_back(__v_arms[i]->arm->iface->finger3());
372 __arms.l.arm->arm->stop();
373 __arms.r.arm->arm->stop();
379 unsigned int first = 0;
380 unsigned int second = 1;
381 if( __v_arms[1]->target->trajec->size() < __v_arms[0]->target->trajec->size() ) {
385 JacoArm* arm_first = __v_arms[first]->arm->arm;
386 JacoArm* arm_second = __v_arms[second]->arm->arm;
387 jaco_trajec_t* trajec_first = *(__v_arms[first]->target->trajec);
388 jaco_trajec_t* trajec_second = *(__v_arms[second]->target->trajec);
389 unsigned int size_first = trajec_first->size();
390 unsigned int size_second = trajec_second->size();
395 for(
unsigned int i=0; i<2; ++i) {
397 cur.push_back( __v_arms[i]->arm->iface->joints(0) );
398 cur.push_back( __v_arms[i]->arm->iface->joints(1) );
399 cur.push_back( __v_arms[i]->arm->iface->joints(2) );
400 cur.push_back( __v_arms[i]->arm->iface->joints(3) );
401 cur.push_back( __v_arms[i]->arm->iface->joints(4) );
402 cur.push_back( __v_arms[i]->arm->iface->joints(5) );
403 __v_arms[i]->arm->arm->goto_joints(cur, __v_arms[i]->target->fingers,
false);
409 while(it < size_first) {
410 arm_first->
goto_joints(trajec_first->at(it), __v_arms[first]->target->fingers,
true);
411 arm_second->
goto_joints(trajec_second->at(it), __v_arms[second]->target->fingers,
true);
416 while(it < size_second) {
417 arm_second->
goto_joints(trajec_second->at(it), __v_arms[second]->target->fingers,
true);
429 JacoBimanualGotoThread::_check_final()
434 for(
unsigned int i=0; i<2; ++i) {
435 switch( __v_arms[i]->target->type ) {
438 for(
unsigned int j=0; j<6; ++j ) {
440 deg2rad(__v_arms[i]->arm->iface->joints(j))) < 0.05;
446 final &= __v_arms[i]->arm->arm->final();
461 for(
unsigned int i=0; i<2; ++i) {
464 if( __v_arms[i]->finger_last[0] == __v_arms[i]->arm->iface->finger1() &&
465 __v_arms[i]->finger_last[1] == __v_arms[i]->arm->iface->finger2() &&
466 __v_arms[i]->finger_last[2] == __v_arms[i]->arm->iface->finger3() ) {
467 __v_arms[i]->finger_last[3] += 1;
469 __v_arms[i]->finger_last[0] = __v_arms[i]->arm->iface->finger1();
470 __v_arms[i]->finger_last[1] = __v_arms[i]->arm->iface->finger2();
471 __v_arms[i]->finger_last[2] = __v_arms[i]->arm->iface->finger3();
472 __v_arms[i]->finger_last[3] = 0;
474 final &= __v_arms[i]->finger_last[3] > 10;
479 __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.