int testJaco() // test main
{
printf("KinDrv example for controlling the arm \n");
// explicitly initialize a libusb context; optional
KinDrv::init_usb();
printf("Create a JacoArm \n");
JacoArm *arm;
try {
arm = new JacoArm();
printf("Successfully connected to arm! \n");
} catch( KinDrvException &e ) {
printf("error %i: %s \n", e.error(), e.what());
return 0;
}
printf("Gaining API control over the arm \n");
arm->start_api_ctrl();
//check if we need to initialize arm
jaco_retract_mode_t mode = arm->get_status();
printf("Arm is currently in state: %i \n", mode);
if( mode == MODE_NOINIT ) {
//push the "HOME/RETRACT" button until arm is initialized
arm->push_joystick_button(2);
while( mode == MODE_NOINIT ) {
std::this_thread::sleep_for(std::chrono::microseconds(10)); // 10 ms
mode = arm->get_status();
}
arm->release_joystick();
}
printf("Arm is initialized now, state: %i \n", mode);
printf("Opening gripper...");
// set control type to angular
arm->set_control_ang();
// current joint values + target finger values
jaco_position_t pos = arm->get_ang_pos();
float finger_open[3] = {0.25, 0.25, 0.25};
// set target values; this moves the arm towards the target position
arm->set_target_ang(pos.joints, finger_open);
// wait a little bit, until movement is finished
std::this_thread::sleep_for(std::chrono::microseconds(2000));
printf("DONE\n");
printf("Closing gripper again...");
float finger_close[3] = {50, 50, 50};
arm->set_target_ang(pos.joints, finger_close);
std::this_thread::sleep_for(std::chrono::microseconds(2000));
printf("DONE\n");
printf("Move arm back to RETRACT position \n");
if( !goto_retract(arm) ) {
printf("Failed to go to RETRACT. Go to HOME first \n");
if( goto_home(arm) ) {
printf("Try RETRACT again\n");
goto_retract(arm);
} else {
printf("Also failed going to HOME. Might be some serious problem...hmm \n");
}
}
// explicitly close libusb context (only needed if explicitly openede before)
KinDrv::close_usb();
return 0;
}
int main()
{
printf("KinDrv example for controlling the arm \n");
// explicitly initialize a libusb context; optional
KinDrv::init_usb();
printf("Create a JacoArm \n");
JacoArm *arm;
try {
arm = new JacoArm();
printf("Successfully connected to arm! \n");
} catch( KinDrvException &e ) {
printf("error %i: %s \n", e.error(), e.what());
return 0;
}
printf("Gaining API control over the arm \n");
arm->start_api_ctrl();
// TEST: Tekin
jaco_position_t ang_pos = arm->get_ang_pos();
printf("angular positions: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_pos.joints[0], ang_pos.joints[1], ang_pos.joints[2],
ang_pos.joints[3], ang_pos.joints[4], ang_pos.joints[5],
ang_pos.finger_position[0], ang_pos.finger_position[1], ang_pos.finger_position[2]);
jaco_position_t cart_pos = arm->get_cart_pos();
printf("cartesian position: x: %f y: %f z: %f Tx: %f Ty: %f Tz: %f fingers: %f %f %f\n",
cart_pos.position[0], cart_pos.position[1], cart_pos.position[2],
cart_pos.rotation[0], cart_pos.rotation[1], cart_pos.rotation[2],
cart_pos.finger_position[0], cart_pos.finger_position[1], cart_pos.finger_position[2]);
jaco_position_t ang_vel = arm->get_ang_vel();
printf("angular velocities: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_vel.joints[0], ang_vel.joints[1], ang_vel.joints[2],
ang_vel.joints[3], ang_vel.joints[4], ang_vel.joints[5],
ang_vel.finger_position[0], ang_vel.finger_position[1], ang_vel.finger_position[2]);
jaco_position_t ang_command = arm->get_ang_command();
printf("angular command: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_command.joints[0], ang_command.joints[1], ang_command.joints[2],
ang_command.joints[3], ang_command.joints[4], ang_command.joints[5],
ang_command.finger_position[0], ang_command.finger_position[1], ang_command.finger_position[2]);
jaco_position_t cart_command = arm->get_cart_command();
printf("cartesian command: x: %f y: %f z: %f Tx: %f Ty: %f Tz: %f fingers: %f %f %f\n",
cart_command.position[0], cart_command.position[1], cart_command.position[2],
cart_command.rotation[0], cart_command.rotation[1], cart_command.rotation[2],
cart_command.finger_position[0], cart_command.finger_position[1], cart_command.finger_position[2]);
jaco_position_t ang_force = arm->get_ang_force();
printf("angular forces: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_force.joints[0], ang_force.joints[1], ang_force.joints[2],
ang_force.joints[3], ang_force.joints[4], ang_force.joints[5],
ang_force.finger_position[0], ang_force.finger_position[1], ang_force.finger_position[2]);
jaco_position_t cart_force = arm->get_cart_force();
printf("cartesian force: x: %f y: %f z: %f Tx: %f Ty: %f Tz: %f fingers: %f %f %f\n",
cart_force.position[0], cart_force.position[1], cart_force.position[2],
cart_force.rotation[0], cart_force.rotation[1], cart_force.rotation[2],
cart_force.finger_position[0], cart_force.finger_position[1], cart_force.finger_position[2]);
jaco_position_t ang_current = arm->get_ang_current();
printf("angular currents: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_current.joints[0], ang_current.joints[1], ang_current.joints[2],
ang_current.joints[3], ang_current.joints[4], ang_current.joints[5],
ang_current.finger_position[0], ang_current.finger_position[1], ang_current.finger_position[2]);
jaco_position_t ang_current_motor = arm->get_ang_current_motor();
printf("angular motor currents: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_current_motor.joints[0], ang_current_motor.joints[1], ang_current_motor.joints[2],
ang_current_motor.joints[3], ang_current_motor.joints[4], ang_current_motor.joints[5],
ang_current_motor.finger_position[0], ang_current_motor.finger_position[1], ang_current_motor.finger_position[2]);
/*
//check if we need to initialize arm
jaco_retract_mode_t mode = arm->get_status();
printf("Arm is currently in state: %i \n", mode);
if( mode == MODE_NOINIT ) {
//push the "HOME/RETRACT" button until arm is initialized
arm->push_joystick_button(2);
while( mode == MODE_NOINIT ) {
usleep(1000*10); // 10 ms
mode = arm->get_status();
}
arm->release_joystick();
}
printf("Arm is initialized now, state: %i \n", mode);
float finger_open[3] = {0, 0, 0};
float finger_close[3] = {5250, 5250, 5250};
// set control type to angular
arm->set_control_ang();
// current joint values + target finger values
jaco_position_t ang_pos = arm->get_ang_pos();
printf("angular: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_pos.joints[0], ang_pos.joints[1], ang_pos.joints[2],
ang_pos.joints[3], ang_pos.joints[4], ang_pos.joints[5],
ang_pos.finger_position[0], ang_pos.finger_position[1], ang_pos.finger_position[2]);
jaco_position_t cart_pos = arm->get_cart_pos();
printf("cartesian: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
cart_pos.joints[0], cart_pos.joints[1], cart_pos.joints[2],
cart_pos.joints[3], cart_pos.joints[4], cart_pos.joints[5],
cart_pos.finger_position[0], cart_pos.finger_position[1], cart_pos.finger_position[2]);
printf("Opening gripper 1...");
arm->set_target_ang(ang_pos.joints, finger_close);
// wait a little bit, until movement is finished
usleep(1000*5000);
printf("DONE\n");
printf("Closing gripper 1...");
arm->set_target_ang(ang_pos.joints, finger_close);
usleep(1000*5000);
printf("DONE\n");
arm->set_target_ang(69.842964, 209.939117, 315.143616, 85.171867, 349.780945, 80.170029, 5250, 5250, 5250);
usleep(1000*5000);
printf("Opening gripper 2...");
// set target values; this moves the arm towards the target position
arm->set_target_ang(ang_pos.joints, finger_open);
// wait a little bit, until movement is finished
usleep(1000*5000);
printf("DONE\n");
printf("Closing gripper 2...");
//float finger_close[3] = {50, 50, 50};
arm->set_target_ang(ang_pos.joints, finger_close);
usleep(1000*2000);
printf("DONE\n");
printf("Move arm back to RETRACT position \n");
if( !goto_retract(arm) ) {
printf("Failed to go to RETRACT. Go to HOME first \n");
if( goto_home(arm) ) {
ang_pos = arm->get_ang_pos();
printf("angular: joints: %f %f %f %f %f %f fingers: %f %f %f\n",
ang_pos.joints[0], ang_pos.joints[1], ang_pos.joints[2],
ang_pos.joints[3], ang_pos.joints[4], ang_pos.joints[5],
ang_pos.finger_position[0], ang_pos.finger_position[1], ang_pos.finger_position[2]);
printf("Try RETRACT again\n");
goto_retract(arm);
} else {
printf("Also failed going to HOME. Might be some serious problem...hmm \n");
}
}
*/
// explicitly close libusb context (only needed if explicitly openede before)
KinDrv::close_usb();
return 0;
}
