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: