23 #include "arm_kindrv.h" 25 #include <core/exception.h> 27 #include <libkindrv/kindrv.h> 47 JacoArmKindrv::JacoArmKindrv(
const char *name)
50 __arm =
new KinDrv::JacoArm();
51 __name = __arm->get_client_config(
true).name;
53 __name.erase(__name.find_last_not_of(
" ")+1);
55 std::string found_names =
"'" + __name +
"'";
59 std::vector<KinDrv::JacoArm*> arms;
60 while( __name.compare(name)!=0 ) {
61 arms.push_back(__arm);
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) {
74 for(
unsigned int i=0; i<arms.size(); ++i) {
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());
85 __initialized =
false;
91 JacoArmKindrv::~JacoArmKindrv()
97 JacoArmKindrv::initialize()
106 JacoArmKindrv::final()
111 switch( __target_type ) {
115 jaco_retract_mode_t mode = __arm->get_status();
116 __final = (mode == MODE_READY_STANDBY);
119 __arm->release_joystick();
120 }
else if( mode == MODE_READY_TO_RETRACT ) {
122 __arm->release_joystick();
123 __arm->push_joystick_button(2);
130 jaco_retract_mode_t mode = __arm->get_status();
131 __final = (mode == MODE_RETRACT_STANDBY);
134 __arm->release_joystick();
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;
144 for(
unsigned int i=0; i<3; ++i ) {
145 __final &= std::abs(vel.finger_position[i]) < 0.01;
155 JacoArmKindrv::initialized()
157 if( !__initialized ) {
158 jaco_retract_mode_t mode = __arm->get_status();
159 __initialized = (mode != MODE_NOINIT);
162 return __initialized;
169 JacoArmKindrv::get_coords(std::vector<float> &to)
173 __arm->set_control_cart();
176 jaco_position_t pos = __arm->get_cart_pos();
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]);
188 JacoArmKindrv::get_joints(std::vector<float> &to)
const 190 jaco_position_t pos = __arm->get_ang_pos();
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]);
202 JacoArmKindrv::get_fingers(std::vector<float> &to)
const 204 jaco_position_t pos = __arm->get_cart_pos();
207 to.push_back(pos.finger_position[0]);
208 to.push_back(pos.finger_position[1]);
209 to.push_back(pos.finger_position[2]);
216 JacoArmKindrv::stop()
218 __arm->release_joystick();
223 JacoArmKindrv::push_joystick(
unsigned int button)
225 __arm->start_api_ctrl();
226 __arm->push_joystick_button(button);
231 JacoArmKindrv::release_joystick()
233 __arm->start_api_ctrl();
234 __arm->release_joystick();
240 JacoArmKindrv::goto_trajec(std::vector< std::vector<float> >* trajec, std::vector<float> &fingers)
242 __arm->start_api_ctrl();
243 __arm->set_control_ang();
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));
254 JacoArmKindrv::goto_joints(std::vector<float> &joints, std::vector<float> &fingers,
bool followup)
260 __arm->start_api_ctrl();
261 __arm->set_control_ang();
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));
271 JacoArmKindrv::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
276 __arm->start_api_ctrl();
277 __arm->set_control_cart();
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));
286 JacoArmKindrv::goto_ready()
291 __arm->start_api_ctrl();
292 jaco_retract_mode_t mode = __arm->get_status();
294 case MODE_RETRACT_TO_READY:
296 __arm->push_joystick_button(2);
297 __arm->release_joystick();
298 __arm->push_joystick_button(2);
301 case MODE_NORMAL_TO_READY:
302 case MODE_READY_TO_RETRACT:
303 case MODE_RETRACT_STANDBY:
307 __arm->push_joystick_button(2);
315 case MODE_READY_STANDBY:
323 JacoArmKindrv::goto_retract()
328 __arm->start_api_ctrl();
329 jaco_retract_mode_t mode = __arm->get_status();
331 case MODE_READY_TO_RETRACT:
333 __arm->push_joystick_button(2);
334 __arm->release_joystick();
335 __arm->push_joystick_button(2);
338 case MODE_READY_STANDBY:
339 case MODE_RETRACT_TO_READY:
341 __arm->push_joystick_button(2);
344 case MODE_NORMAL_TO_READY:
356 case MODE_RETRACT_STANDBY:
Fawkes library namespace.
Base class for exceptions in Fawkes.
target with cartesian coordinates.
target with angular coordinates.
target is the RETRACT position of the Jaco arm.
target is the READY position of the Jaco arm.