Fawkes API  Fawkes Development Version
arm_kindrv.cpp
1 
2 /***************************************************************************
3  * arm_kindrv.cpp - Class for a Kinova Jaco arm, using libkindrv
4  *
5  * Created: Tue Jul 29 14:58:32 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 #include "arm_kindrv.h"
24 
25 #include <core/exception.h>
26 
27 #include <libkindrv/kindrv.h>
28 
29 #include <cstdio>
30 #include <vector>
31 
32 using namespace KinDrv;
33 
34 namespace fawkes {
35 #if 0 /* just to make Emacs auto-indent happy */
36 }
37 #endif
38 
39 /** @class JacoArmKindrv <plugins/jaco/arm_kindrv.h>
40  * Class for commanding a Kinova Jaco Arm, using libkindrv.
41  * @author Bahram Maleki-Fard
42  */
43 
44 /** Constructor.
45  * @param name The name of the arm we want to connect to.
46  */
47 JacoArmKindrv::JacoArmKindrv(const char *name)
48 {
49  // take the first arm we can connect to
50  __arm = new KinDrv::JacoArm();
51  __name = __arm->get_client_config(true).name;
52  // trim tailing whitespaces
53  __name.erase(__name.find_last_not_of(" ")+1);
54 
55  std::string found_names = "'" + __name + "'";
56 
57  if( name!=NULL ) {
58  // Check all connected arms until the right one is found.
59  std::vector<KinDrv::JacoArm*> arms;
60  while( __name.compare(name)!=0 ) {
61  arms.push_back(__arm);
62  try {
63  __arm = new KinDrv::JacoArm();
64  __name = __arm->get_client_config(true).name;
65  __name.erase(__name.find_last_not_of(" ")+1);
66  found_names += ", '" + __name + "'";
67  } catch(KinDrvException& e) {
68  // don't throw yet, we need to delete the occupied arms first.
69  __arm = NULL;
70  break;
71  }
72  }
73 
74  for(unsigned int i=0; i<arms.size(); ++i) {
75  delete arms[i];
76  arms[i] = NULL;
77  }
78  }
79 
80  if( __arm==NULL )
81  {
82  throw fawkes::Exception("Could not connect to Jaco arm '%s' with libkindrv. But I found the following arms: %s", name, found_names.c_str());
83  }
84 
85  __initialized = false;
86  __final = true;
87  __ctrl_ang = true;
88 }
89 
90 /** Destructor. */
91 JacoArmKindrv::~JacoArmKindrv()
92 {
93  delete(__arm);
94 }
95 
96 void
97 JacoArmKindrv::initialize()
98 {
99  goto_ready();
100 }
101 
102 
103 
104 
105 bool
106 JacoArmKindrv::final()
107 {
108  if( __final )
109  return true;
110 
111  switch( __target_type ) {
112 
113  case TARGET_READY:
114  {
115  jaco_retract_mode_t mode = __arm->get_status();
116  __final = (mode == MODE_READY_STANDBY);
117 
118  if( __final ) {
119  __arm->release_joystick();
120  } else if( mode == MODE_READY_TO_RETRACT ) {
121  // is moving in wrong direction
122  __arm->release_joystick();
123  __arm->push_joystick_button(2);
124  }
125  }
126  break;
127 
128  case TARGET_RETRACT:
129  {
130  jaco_retract_mode_t mode = __arm->get_status();
131  __final = (mode == MODE_RETRACT_STANDBY);
132  }
133  if( __final )
134  __arm->release_joystick();
135  break;
136 
137  default: //TARGET_ANGULAR, TARGET_CARTESIAN
138  __final = true;
139  {
140  jaco_position_t vel = __arm->get_ang_vel();
141  for( unsigned int i=0; i<6; ++i ) {
142  __final &= std::abs(vel.joints[i]) < 0.01;
143  }
144  for( unsigned int i=0; i<3; ++i ) {
145  __final &= std::abs(vel.finger_position[i]) < 0.01;
146  }
147  }
148  break;
149  }
150 
151  return __final;
152 }
153 
154 bool
155 JacoArmKindrv::initialized()
156 {
157  if( !__initialized ) {
158  jaco_retract_mode_t mode = __arm->get_status();
159  __initialized = (mode != MODE_NOINIT);
160  }
161 
162  return __initialized;
163 }
164 
165 
166 
167 
168 void
169 JacoArmKindrv::get_coords(std::vector<float> &to)
170 {
171  if( __ctrl_ang ) {
172  // nedd to set control to cart, otherwise we will not get updated data
173  __arm->set_control_cart();
174  __ctrl_ang = false;
175  }
176  jaco_position_t pos = __arm->get_cart_pos();
177 
178  to.clear();
179  to.push_back(-pos.position[1]);
180  to.push_back( pos.position[0]);
181  to.push_back( pos.position[2]);
182  to.push_back(pos.rotation[0]);
183  to.push_back(pos.rotation[1]);
184  to.push_back(pos.rotation[2]);
185 }
186 
187 void
188 JacoArmKindrv::get_joints(std::vector<float> &to) const
189 {
190  jaco_position_t pos = __arm->get_ang_pos();
191 
192  to.clear();
193  to.push_back(pos.joints[0]);
194  to.push_back(pos.joints[1]);
195  to.push_back(pos.joints[2]);
196  to.push_back(pos.joints[3]);
197  to.push_back(pos.joints[4]);
198  to.push_back(pos.joints[5]);
199 }
200 
201 void
202 JacoArmKindrv::get_fingers(std::vector<float> &to) const
203 {
204  jaco_position_t pos = __arm->get_cart_pos();
205 
206  to.clear();
207  to.push_back(pos.finger_position[0]);
208  to.push_back(pos.finger_position[1]);
209  to.push_back(pos.finger_position[2]);
210 }
211 
212 
213 
214 
215 void
216 JacoArmKindrv::stop()
217 {
218  __arm->release_joystick();
219  __final = true;
220 }
221 
222 void
223 JacoArmKindrv::push_joystick(unsigned int button)
224 {
225  __arm->start_api_ctrl();
226  __arm->push_joystick_button(button);
227  __final = false;
228 }
229 
230 void
231 JacoArmKindrv::release_joystick()
232 {
233  __arm->start_api_ctrl();
234  __arm->release_joystick();
235  __final = true;
236 }
237 
238 
239 void
240 JacoArmKindrv::goto_trajec(std::vector< std::vector<float> >* trajec, std::vector<float> &fingers)
241 {
242  __arm->start_api_ctrl();
243  __arm->set_control_ang();
244  __ctrl_ang = true;
245  usleep(500);
246  for( unsigned int i=0; i<trajec->size(); ++i ) {
247  __arm->set_target_ang(trajec->at(i).at(0), trajec->at(i).at(1), trajec->at(i).at(2),
248  trajec->at(i).at(3), trajec->at(i).at(4), trajec->at(i).at(5),
249  fingers.at(0), fingers.at(1), fingers.at(2));
250  }
251 }
252 
253 void
254 JacoArmKindrv::goto_joints(std::vector<float> &joints, std::vector<float> &fingers, bool followup)
255 {
256  __target_type = TARGET_ANGULAR;
257  __final = false;
258 
259  if(!followup) {
260  __arm->start_api_ctrl();
261  __arm->set_control_ang();
262  __ctrl_ang = true;
263  usleep(500);
264  }
265 
266  __arm->set_target_ang(joints.at(0), joints.at(1), joints.at(2), joints.at(3), joints.at(4), joints.at(5),
267  fingers.at(0), fingers.at(1), fingers.at(2));
268 }
269 
270 void
271 JacoArmKindrv::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
272 {
273  __target_type = TARGET_CARTESIAN;
274  __final = false;
275 
276  __arm->start_api_ctrl();
277  __arm->set_control_cart();
278  __ctrl_ang = false;
279  usleep(500);
280  //__arm->arm->set_target_cart(__y, -__x, __z, __e1, __e2, __e3, __f1, __f2, __f3);
281  __arm->set_target_cart(coords.at(1), -coords.at(0), coords.at(2), coords.at(3), coords.at(4), coords.at(5),
282  fingers.at(0), fingers.at(1), fingers.at(2));
283 }
284 
285 void
286 JacoArmKindrv::goto_ready()
287 {
288  __target_type = TARGET_READY;
289  __final = false;
290 
291  __arm->start_api_ctrl();
292  jaco_retract_mode_t mode = __arm->get_status();
293  switch( mode ) {
294  case MODE_RETRACT_TO_READY:
295  //2 buttons needed
296  __arm->push_joystick_button(2);
297  __arm->release_joystick();
298  __arm->push_joystick_button(2);
299  break;
300 
301  case MODE_NORMAL_TO_READY:
302  case MODE_READY_TO_RETRACT:
303  case MODE_RETRACT_STANDBY:
304  case MODE_NORMAL:
305  case MODE_NOINIT:
306  //1 button needed
307  __arm->push_joystick_button(2);
308  break;
309 
310  case MODE_ERROR:
311  // error: some error occured
312  // TODO: return something?
313  break;
314 
315  case MODE_READY_STANDBY:
316  // no action. error?
317  // __final = true;
318  break;
319  }
320 }
321 
322 void
323 JacoArmKindrv::goto_retract()
324 {
325  __target_type = TARGET_RETRACT;
326  __final = false;
327 
328  __arm->start_api_ctrl();
329  jaco_retract_mode_t mode = __arm->get_status();
330  switch( mode ) {
331  case MODE_READY_TO_RETRACT:
332  // 2 buttons needed
333  __arm->push_joystick_button(2);
334  __arm->release_joystick();
335  __arm->push_joystick_button(2);
336  break;
337 
338  case MODE_READY_STANDBY:
339  case MODE_RETRACT_TO_READY:
340  // 1 button needed
341  __arm->push_joystick_button(2);
342  break;
343 
344  case MODE_NORMAL_TO_READY:
345  case MODE_NORMAL:
346  case MODE_NOINIT:
347  // warn: cannot go from NORMAL/NOINIT to RETRACT");
348  //__final = true;
349  break;
350 
351  case MODE_ERROR:
352  // error: some error occured!!
353  // TODO: return something?
354  break;
355 
356  case MODE_RETRACT_STANDBY:
357  // no action. error?
358  //__final = true;
359  break;
360  }
361 }
362 
363 } // end of namespace fawkes
Fawkes library namespace.
Base class for exceptions in Fawkes.
Definition: exception.h:36
target with cartesian coordinates.
Definition: types.h:62
target with angular coordinates.
Definition: types.h:63
target is the RETRACT position of the Jaco arm.
Definition: types.h:66
target is the READY position of the Jaco arm.
Definition: types.h:65