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