Fawkes API  Fawkes Development Version
bimanual_goto_thread.h
1 
2 /***************************************************************************
3  * bimaual_goto_thread.h - Jaco plugin movement thread for coordinated bimanual manipulation
4  *
5  * Created: Mon Sep 29 23:17:12 2014
6  * Copyright 2014 Bahram Maleki-Fard
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef _PLUGINS_JACO_BIMANUAL_GOTO_THREAD_H_
24 #define _PLUGINS_JACO_BIMANUAL_GOTO_THREAD_H_
25 
26 #include "types.h"
27 
28 #include <aspect/blackboard.h>
29 #include <aspect/configurable.h>
30 #include <aspect/logging.h>
31 #include <core/threading/thread.h>
32 
33 namespace fawkes {
34 class Mutex;
35 }
37  public fawkes::LoggingAspect,
40 {
41 public:
43  virtual ~JacoBimanualGotoThread();
44 
45  virtual void init();
46  virtual void finalize();
47  virtual void loop();
48 
49  virtual bool final();
50 
51  virtual void stop();
52  virtual void move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3);
53 
54  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
55 protected:
56  virtual void
57  run()
58  {
59  Thread::run();
60  }
61 
62 private:
63  void _lock_queues() const;
64  void _unlock_queues() const;
65  void _enqueue_targets(fawkes::RefPtr<fawkes::jaco_target_t> l,
67 
68  void _move_grippers();
69  void _exec_trajecs();
70 
71  void _check_final();
72 
73  typedef struct arm_struct
74  {
75  fawkes::jaco_arm_t * arm;
77  float finger_last[4]; // 3 positions + 1 counter
78  } arm_struct_t;
79 
80  struct
81  {
82  arm_struct_t l;
83  arm_struct_t r;
84  } arms_;
85 
86  arm_struct_t *v_arms_[2]; // just a helper, to be able to iterate over both arms
87 
88  fawkes::jaco_dual_arm_t *dual_arms_; // have redundancy now, but keep this just to be sure
89 
90  fawkes::Mutex *final_mutex_;
91  bool final_;
92 };
93 
94 #endif
Jaco struct containing all components required for one arm.
Definition: types.h:92
Thread aspect to access to BlackBoard.
Definition: blackboard.h:33
virtual void finalize()
Finalize the thread.
Jaco Arm movement thread.
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
Fawkes library namespace.
virtual void loop()
The main loop of this thread.
virtual ~JacoBimanualGotoThread()
Destructor.
Thread class encapsulation of pthreads.
Definition: thread.h:45
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void init()
Initialize the thread.
Thread aspect to log output.
Definition: logging.h:32
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.
Thread aspect to access configuration data.
Definition: configurable.h:32
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
Mutex mutual exclusion lock.
Definition: mutex.h:32