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