23#include "arm_kindrv.h"
25#include <core/exception.h>
26#include <libkindrv/kindrv.h>
31using namespace KinDrv;
46 arm_.reset(
new KinDrv::JacoArm);
47 name_ = arm_->get_client_config(
true).name;
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;
62 found_names +=
", '" +
name_ +
"'";
63 }
catch (KinDrvException &e) {
74 "But I found the following arms: %s",
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;
144 jaco_retract_mode_t mode = arm_->get_status();
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]);
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]);
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]);
198 arm_->release_joystick();
205 arm_->start_api_ctrl();
206 arm_->push_joystick_button(button);
213 arm_->start_api_ctrl();
214 arm_->release_joystick();
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),
245 arm_->start_api_ctrl();
246 arm_->set_control_ang();
251 arm_->set_target_ang(joints.at(0),
268 arm_->start_api_ctrl();
269 arm_->set_control_cart();
273 arm_->set_target_cart(coords.at(1),
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:
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:
Base class for exceptions in Fawkes.
virtual void get_fingers(std::vector< float > &to) const
Get the position values of the fingers.
virtual void goto_trajec(std::vector< std::vector< float > > *trajec, std::vector< float > &fingers)
Move the arm along the given trajectory.
virtual bool final()
Check if movement is final.
virtual void release_joystick()
Simulate releasing the joystick of the Kinova Jaco arm.
virtual void goto_ready()
Move the arm to READY position.
virtual void stop()
Stop the current movement.
JacoArmKindrv(const char *name=NULL)
Constructor.
virtual ~JacoArmKindrv()
Destructor.
virtual void initialize()
Initialize the arm.
virtual void push_joystick(unsigned int button)
Simulate a push of a button on the joystick of the Kinova Jaco arm.
virtual void goto_retract()
Move the arm to RETRACT position.
virtual bool initialized()
Check if arm is initialized.
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)
Move the arm to given configuration.
virtual void get_coords(std::vector< float > &to)
Get the cartesian coordinates of the arm.
virtual void get_joints(std::vector< float > &to) const
Get the joint angles of the arm.
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)
Move the arm to given configuration.
std::string name_
the name of this arm
bool initialized_
track if the arm has been initialized or not
Fawkes library namespace.
@ TARGET_READY
target is the READY position of the Jaco arm.
@ TARGET_CARTESIAN
target with cartesian coordinates.
@ TARGET_RETRACT
target is the RETRACT position of the Jaco arm.
@ TARGET_ANGULAR
target with angular coordinates.