23 #include "arm_kindrv.h" 25 #include <core/exception.h> 26 #include <libkindrv/kindrv.h> 31 using namespace KinDrv;
43 JacoArmKindrv::JacoArmKindrv(
const char *name)
46 arm_.reset(
new KinDrv::JacoArm);
47 name_ = arm_->get_client_config(
true).name;
49 name_.erase(name_.find_last_not_of(
" ") + 1);
51 std::string found_names =
"'" + name_ +
"'";
55 std::vector<std::unique_ptr<KinDrv::JacoArm>> arms;
56 while (name_.compare(name) != 0) {
57 arms.push_back(std::move(arm_));
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) {
74 "But I found the following arms: %s",
85 JacoArmKindrv::~JacoArmKindrv()
90 JacoArmKindrv::initialize()
96 JacoArmKindrv::final()
101 switch (target_type_) {
103 jaco_retract_mode_t mode = arm_->get_status();
104 final_ = (mode == MODE_READY_STANDBY);
107 arm_->release_joystick();
108 }
else if (mode == MODE_READY_TO_RETRACT) {
110 arm_->release_joystick();
111 arm_->push_joystick_button(2);
116 jaco_retract_mode_t mode = arm_->get_status();
117 final_ = (mode == MODE_RETRACT_STANDBY);
120 arm_->release_joystick();
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;
130 for (
unsigned int i = 0; i < 3; ++i) {
131 final_ &= std::abs(vel.finger_position[i]) < 0.01;
141 JacoArmKindrv::initialized()
144 jaco_retract_mode_t mode = arm_->get_status();
145 initialized_ = (mode != MODE_NOINIT);
152 JacoArmKindrv::get_coords(std::vector<float> &to)
156 arm_->set_control_cart();
159 jaco_position_t pos = arm_->get_cart_pos();
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]);
171 JacoArmKindrv::get_joints(std::vector<float> &to)
const 173 jaco_position_t pos = arm_->get_ang_pos();
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]);
185 JacoArmKindrv::get_fingers(std::vector<float> &to)
const 187 jaco_position_t pos = arm_->get_cart_pos();
190 to.push_back(pos.finger_position[0]);
191 to.push_back(pos.finger_position[1]);
192 to.push_back(pos.finger_position[2]);
196 JacoArmKindrv::stop()
198 arm_->release_joystick();
203 JacoArmKindrv::push_joystick(
unsigned int button)
205 arm_->start_api_ctrl();
206 arm_->push_joystick_button(button);
211 JacoArmKindrv::release_joystick()
213 arm_->start_api_ctrl();
214 arm_->release_joystick();
219 JacoArmKindrv::goto_trajec(std::vector<std::vector<float>> *trajec, std::vector<float> &fingers)
221 arm_->start_api_ctrl();
222 arm_->set_control_ang();
225 for (
unsigned int i = 0; i < trajec->size(); ++i) {
226 arm_->set_target_ang(trajec->at(i).at(0),
239 JacoArmKindrv::goto_joints(std::vector<float> &joints, std::vector<float> &fingers,
bool followup)
245 arm_->start_api_ctrl();
246 arm_->set_control_ang();
251 arm_->set_target_ang(joints.at(0),
263 JacoArmKindrv::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
268 arm_->start_api_ctrl();
269 arm_->set_control_cart();
273 arm_->set_target_cart(coords.at(1),
285 JacoArmKindrv::goto_ready()
290 arm_->start_api_ctrl();
291 jaco_retract_mode_t mode = arm_->get_status();
293 case MODE_RETRACT_TO_READY:
295 arm_->push_joystick_button(2);
296 arm_->release_joystick();
297 arm_->push_joystick_button(2);
300 case MODE_NORMAL_TO_READY:
301 case MODE_READY_TO_RETRACT:
302 case MODE_RETRACT_STANDBY:
306 arm_->push_joystick_button(2);
314 case MODE_READY_STANDBY:
322 JacoArmKindrv::goto_retract()
327 arm_->start_api_ctrl();
328 jaco_retract_mode_t mode = arm_->get_status();
330 case MODE_READY_TO_RETRACT:
332 arm_->push_joystick_button(2);
333 arm_->release_joystick();
334 arm_->push_joystick_button(2);
337 case MODE_READY_STANDBY:
338 case MODE_RETRACT_TO_READY:
340 arm_->push_joystick_button(2);
343 case MODE_NORMAL_TO_READY:
355 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.