Fawkes API  Fawkes Development Version
jaco_plugin.cpp
1 
2 /***************************************************************************
3  * jaco_plugin.cpp - Fawkes Kinova Jaco Plugin
4  *
5  * Created: Tue Jun 04 13:13:20 2013
6  * Copyright 2013 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 #include "jaco_plugin.h"
24 
25 #include "info_thread.h"
26 #include "act_thread.h"
27 #include "goto_thread.h"
28 #include "openrave_thread.h"
29 
30 #include "bimanual_act_thread.h"
31 #include "bimanual_goto_thread.h"
32 #include "bimanual_openrave_thread.h"
33 
34 #include "types.h"
35 
36 using namespace fawkes;
37 
38 /** @class JacoPlugin <plugins/jaco/jaco_plugin.h>
39  * Kinova Jaco Arm plugin.
40  *
41  * @author Bahram Maleki-Fard
42  */
43 
44 /** Constructor.
45  * @param config Fawkes configuration
46  */
48  : Plugin(config)
49 {
50  // load different/multiple threads if using dual-arm setup
51  bool is_dual_arm = config->get_bool("/hardware/jaco/config/dual_arm");
52  if( !is_dual_arm ) {
53  jaco_arm_t* arm = new jaco_arm_t();
54  arm->config=CONFIG_SINGLE;
55 
56  JacoActThread* act_thread = new JacoActThread("JacoActThread", arm);
57  JacoInfoThread* info_thread = new JacoInfoThread("JacoInfoThread", arm);
58  JacoGotoThread* goto_thread = new JacoGotoThread("JacoGotoThread", arm);
59  JacoOpenraveThread* openrave_thread = NULL;
60 #ifdef HAVE_OPENRAVE
61  openrave_thread = new JacoOpenraveThread("JacoOpenraveThread", arm);
62 #endif
63 
64  arm->goto_thread = goto_thread;
65  arm->openrave_thread = openrave_thread;
66 
67  thread_list.push_back( act_thread );
68  thread_list.push_back( info_thread );
69  thread_list.push_back( goto_thread );
70 #ifdef HAVE_OPENRAVE
71  thread_list.push_back( openrave_thread );
72 #endif
73 
74  } else {
75  jaco_arm_t* arm_l = new jaco_arm_t();
76  jaco_arm_t* arm_r = new jaco_arm_t();
77  arm_l->config=CONFIG_LEFT;
78  arm_r->config=CONFIG_RIGHT;
79 
80 
81  // each arm gets a separate set of threads for independent manipulation.
82  JacoActThread* act_thread_l = new JacoActThread("JacoActThreadLeft", arm_l);
83  JacoInfoThread* info_thread_l = new JacoInfoThread("JacoInfoThreadLeft", arm_l);
84  JacoGotoThread* goto_thread_l = new JacoGotoThread("JacoGotoThreadLeft", arm_l);
85 
86  JacoActThread* act_thread_r = new JacoActThread("JacoActThreadRight", arm_r);
87  JacoInfoThread* info_thread_r = new JacoInfoThread("JacoInfoThreadRight", arm_r);
88  JacoGotoThread* goto_thread_r = new JacoGotoThread("JacoGotoThreadRight", arm_r);
89 
90  JacoOpenraveThread* openrave_thread_l = NULL;
91  JacoOpenraveThread* openrave_thread_r = NULL;
92 #ifdef HAVE_OPENRAVE
93  // each arm gets 1 openrave-thread, providing planning and updating the openrave-model of that arm.
94  // additionally we need a separate openrave thread for planning symmetric bimanual manipulation.
95  openrave_thread_l = new JacoOpenraveThread("JacoOpenraveThreadLeft", arm_l, /*load_robot=*/false);
96  openrave_thread_r = new JacoOpenraveThread("JacoOpenraveThreadRight", arm_r, /*load_robot=*/false);
97 #endif
98 
99  arm_l->goto_thread = goto_thread_l;
100  arm_l->openrave_thread = openrave_thread_l;
101 
102  arm_r->goto_thread = goto_thread_r;
103  arm_r->openrave_thread = openrave_thread_r;
104 
105  thread_list.push_back( act_thread_l );
106  thread_list.push_back( info_thread_l );
107  thread_list.push_back( goto_thread_l );
108 
109  thread_list.push_back( act_thread_r );
110  thread_list.push_back( info_thread_r );
111  thread_list.push_back( goto_thread_r );
112 
113  // we also need a set of threads for coordinated bimanual manipulation
114  jaco_dual_arm_t* arms = new jaco_dual_arm_t();
115  arms->left = arm_l;
116  arms->right = arm_r;
117 
118  JacoBimanualActThread* act_thread = new JacoBimanualActThread(arms);
119  JacoBimanualGotoThread* goto_thread = new JacoBimanualGotoThread(arms);
120 
121  JacoBimanualOpenraveThread* openrave_thread = NULL;
122 #ifdef HAVE_OPENRAVE
123  openrave_thread = new JacoBimanualOpenraveThread(arms);
124 #endif
125 
126  arms->goto_thread = goto_thread;
127  arms->openrave_thread = openrave_thread;
128 
129  thread_list.push_back( act_thread );
130  thread_list.push_back( goto_thread );
131 #ifdef HAVE_OPENRAVE
132  thread_list.push_back( openrave_thread );
133  thread_list.push_back( openrave_thread_l );
134  thread_list.push_back( openrave_thread_r );
135 #endif
136  }
137 }
138 
139 PLUGIN_DESCRIPTION("Jaco Plugin")
140 EXPORT_PLUGIN(JacoPlugin)
JacoGotoThread * goto_thread
the GotoThread of this arm.
Definition: types.h:100
Jaco struct containing all components required for one arm.
Definition: types.h:95
Plugin interface class.
Definition: plugin.h:33
Jaco Arm act-thread for coordinate bimanual manipulation.
Jaco Arm thread for dual-arm setup, integrating OpenRAVE.
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
Definition: types.h:116
Jaco Arm movement thread.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
jaco_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:113
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.
Definition: types.h:117
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:114
this arm is the right one out of two.
Definition: types.h:57
JacoPlugin(fawkes::Configuration *config)
Constructor.
Definition: jaco_plugin.cpp:47
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:101
Jaco Arm control thread.
Definition: act_thread.h:36
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
jaco_arm_config_t config
configuration for this arm
Definition: types.h:96
Jaco Arm information thread.
Definition: info_thread.h:38
Jaco Arm movement thread.
Definition: goto_thread.h:40
void push_back(Thread *thread)
Add thread to the end.
this arm is the left one out of two.
Definition: types.h:56
Jaco Arm thread for single-arm setup, integrating OpenRAVE.
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
Interface for configuration handling.
Definition: config.h:67
we only have one arm.
Definition: types.h:55
Kinova Jaco Arm plugin.
Definition: jaco_plugin.h:28