You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
681 lines
19 KiB
681 lines
19 KiB
/*
|
|
* VisualServoing is a tutorial program for introducing students to
|
|
* robotics.
|
|
*
|
|
* Copyright 2009, 2010 Kevin Quigley <kevin.quigley@gmail.com> and
|
|
* Marsette Vona <vona@ccs.neu.edu>
|
|
*
|
|
* VisualServoing is free software: you can redistribute it andor modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* VisualServoing is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* as the file COPYING along with VisualServoing. If not, see
|
|
* <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
// system headers
|
|
#include <cstdio>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <signal.h>
|
|
#include <unistd.h>
|
|
|
|
// CTY arm project
|
|
#include "ArmControl.hpp"
|
|
#include "ArmGui.hpp"
|
|
#include "ArmGuiGTK.hpp"
|
|
#include "IK.hpp"
|
|
#include "ImageProcessing.hpp"
|
|
#include "Params.hpp"
|
|
|
|
// OpenCV
|
|
#include <cv.h>
|
|
#include <highgui.h>
|
|
|
|
// Boost
|
|
#include <boost/numeric/ublas/vector.hpp>
|
|
#include <boost/numeric/ublas/io.hpp>
|
|
|
|
using namespace boost::numeric;
|
|
|
|
#if RUN_THREADED
|
|
#include <errno.h>
|
|
#include <pthread.h>
|
|
#endif
|
|
|
|
// constants
|
|
#define FOCAL_LENGTH 481.0 // calc p65 //TBD - add calculation info
|
|
#define DIAMETER .038 //!< Diameter of ball in meters (measurexd)
|
|
#define MIN_TRACKING_RADIUS_PIXELS 2.0 //!< Minimum tracking radius required
|
|
|
|
void mark_images(const ublas::vector<double>& target, const CvSeq* circles,
|
|
const Params& params, Images& images);
|
|
|
|
void calibrate_offsets(std::string& file, ublas::vector<double>& offsets);
|
|
void update_gui_position (ArmControl& ctl, Params& params);
|
|
|
|
void handler(int sig);
|
|
ArmControl* sig_ctl = 0; //!< Pointer to ArmControl for stopping arm
|
|
//! movement upon received signal.
|
|
|
|
|
|
/*!
|
|
* \Brief Starting function containing main control loop.
|
|
* Start up and configure all objects, spin off the GUI, and continue
|
|
* in main loop until told to stop.
|
|
*/
|
|
int main(int argc, char** argv) {
|
|
// set signal handling
|
|
struct sigaction action;
|
|
action.sa_handler = &handler;
|
|
|
|
if (sigaction(SIGHUP, &action, NULL) < 0)
|
|
printf("Error setting action for SIGHUP\n");
|
|
if (sigaction(SIGINT, &action, NULL) < 0)
|
|
printf("Error setting action for SIGINT\n");
|
|
if (sigaction(SIGQUIT, &action, NULL) < 0)
|
|
printf("Error setting action for SIGQUIT\n");
|
|
if (sigaction(SIGILL, &action, NULL) < 0)
|
|
printf("Error setting action for SIGILL\n");
|
|
if (sigaction(SIGABRT, &action, NULL) < 0)
|
|
printf("Error setting action for SIGABRT\n");
|
|
if (sigaction(SIGFPE, &action, NULL) < 0)
|
|
printf("Error setting action for SIGFPE\n");
|
|
if (sigaction(SIGSEGV, &action, NULL) < 0)
|
|
printf("Error setting action for SIGSEGV\n");
|
|
if (sigaction(SIGTERM, &action, NULL) < 0)
|
|
printf("Error setting action for SIGTERM\n");
|
|
|
|
Images images;
|
|
images.set = false;
|
|
images.bgr = 0;
|
|
images.filtered_bgr = 0;
|
|
images.filtered_hls = 0;
|
|
|
|
Params params;
|
|
init_params(params);
|
|
CvSeq* circles = 0;
|
|
|
|
unsigned int cameraID(0);
|
|
std::string port("/dev/ttyS0");
|
|
std::string config_file;
|
|
|
|
std::string flags = "hp:f:";
|
|
int opt;
|
|
bool help = false;
|
|
while ((opt = getopt(argc, argv, flags.c_str())) > 0) {
|
|
switch (opt) {
|
|
case 'h': help = true; break;
|
|
case 'p': port = optarg; break;
|
|
case 'f': config_file = optarg; break;
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
if (help) {
|
|
printf("Visual Servo Arm Options:\n"
|
|
" -h Print this help menu\n"
|
|
" -f <file> Use a calibration file to set joint offsets\n"
|
|
" -p <port> Use an alternate serial port (default: /dev/ttyS0\n");
|
|
exit(0);
|
|
}
|
|
|
|
CvCapture* capture(0);
|
|
IplImage* frame(0);
|
|
|
|
ImageProcessing ip;
|
|
|
|
ublas::vector<double> features(3);
|
|
ublas::vector<double> delta_angles(3);
|
|
ublas::vector<double> target_pos(3);
|
|
ublas::vector<double> grab_target(3);
|
|
|
|
target_pos(0) = 0.0; //x
|
|
target_pos(1) = 0.0; //y
|
|
target_pos(2) = 0.2; //Z
|
|
|
|
grab_target(0) = 0.0; //x
|
|
grab_target(1) = 0.0; //y
|
|
grab_target(2) = 0.05; //Z
|
|
|
|
// div by focal_length to normalize target x,y
|
|
ublas::vector<double> target_pos_norm(target_pos);
|
|
target_pos_norm(0) /= FOCAL_LENGTH;
|
|
target_pos_norm(1) /= FOCAL_LENGTH;
|
|
|
|
IK ik;
|
|
ik.setTarget(target_pos_norm);
|
|
ik.setLengths(0.0, .152, 0.122, 0.075);
|
|
ik.setV(.015, -.150, .25); //m, m, rad
|
|
|
|
ArmGuiGTK* gui = ArmGuiGTK::instance();
|
|
gui->update(images, params);
|
|
|
|
#if RUN_THREADED
|
|
pthread_t guiTID;
|
|
switch (pthread_create(&guiTID, 0, ArmGui::threadRun, gui)) {
|
|
case EAGAIN: printf("Max threads reached\n"); return -1;
|
|
case EINVAL: printf("Invalid thread attributes\n"); return -1;
|
|
case EPERM: printf("Invalid permissions\n"); return -1;
|
|
default: break;
|
|
}
|
|
#endif
|
|
|
|
SSC32Controller ssc(port);
|
|
ArmControl ctl(ssc);
|
|
sig_ctl = &ctl;
|
|
ctl.setRateLimit(500);
|
|
|
|
ublas::vector<double> off(ublas::zero_vector<double>(NUM_JOINTS));
|
|
calibrate_offsets(config_file, off);
|
|
ctl.setOffset(off);
|
|
|
|
ublas::vector<double> angle_limits(ublas::vector<double>(NUM_JOINTS));
|
|
|
|
// max limits
|
|
angle_limits(0) = 3.0/8.0 * M_PI;
|
|
angle_limits(1) = M_PI_2;
|
|
angle_limits(2) = M_PI - .70; // off arm brace
|
|
angle_limits(3) = M_PI_2;
|
|
std::cout << "max limits: " << angle_limits << std::endl;
|
|
ctl.setMaxAngle(angle_limits);
|
|
|
|
ArmControl::radiansToDegrees(angle_limits);
|
|
for (int i = 0; i < NUM_JOINTS; i++)
|
|
params.ctl.max_limits[i] = angle_limits(i);
|
|
params.limits_changed = true;
|
|
|
|
// min limits
|
|
angle_limits(0) = -3.0/8.0 * M_PI;
|
|
angle_limits(1) = -M_PI_2 + 0.35; // off spring pedestal
|
|
// angle_limits(2) = 0;
|
|
angle_limits(2) = -50.0*2.0*M_PI/360.0;
|
|
angle_limits(3) = -M_PI_2;
|
|
ctl.setMinAngle(angle_limits);
|
|
std::cout << "min limits: " << angle_limits << std::endl;
|
|
|
|
ArmControl::radiansToDegrees(angle_limits);
|
|
|
|
for (int i = 0; i < NUM_JOINTS; i++)
|
|
params.ctl.min_limits[i] = angle_limits(i);
|
|
params.limits_changed = true;
|
|
|
|
ctl.park();
|
|
update_gui_position(ctl, params);
|
|
params.current_mode = PARK;
|
|
|
|
|
|
while (params.run) { //mainloop
|
|
|
|
gui->update(images, params);
|
|
|
|
#if !RUN_THREADED
|
|
gui->run();
|
|
#endif
|
|
|
|
if (!params.run) continue; //to next mainloop iteration
|
|
|
|
if (params.gui.estop) {
|
|
params.gui.estop = false;
|
|
printf("ESTOP received\n");
|
|
ctl.stop();
|
|
if (params.current_mode != ESTOP) {
|
|
params.current_mode = ESTOP;
|
|
}
|
|
}
|
|
|
|
// all activities respond to these new modes
|
|
switch (params.new_mode) {
|
|
case HOME:
|
|
params.new_mode = NONE;
|
|
printf("*** -> HOME\n");
|
|
ctl.home();
|
|
update_gui_position(ctl, params);
|
|
params.current_mode = READY;
|
|
break;
|
|
case PARK:
|
|
printf("park request\n");
|
|
params.new_mode = NONE;
|
|
printf("*** -> PARK\n");
|
|
ctl.park();
|
|
update_gui_position(ctl, params);
|
|
params.current_mode = PARK;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// all activities respond to these current modes
|
|
switch (params.current_mode) {
|
|
case HOME:
|
|
printf("HOME->READY\n");
|
|
params.current_mode = READY;
|
|
break;
|
|
case PARK:
|
|
// getting out of PARK handled above
|
|
usleep(10000); // 10ms
|
|
case BUSY:
|
|
printf("BUSY -> READY\n");
|
|
if (!ctl.busy())
|
|
params.current_mode = READY;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (params.activity == KINEMATICS) {
|
|
|
|
usleep(10000); // 10ms
|
|
|
|
ctl.slaveWrist(false);
|
|
ublas::vector<double> new_position(NUM_JOINTS);
|
|
|
|
if (params.current_mode == READY) {
|
|
switch (params.new_mode) {
|
|
case MOVE:
|
|
params.new_mode = NONE;
|
|
printf("Moving\n");
|
|
for (int i = 0; i < NUM_JOINTS; i++ )
|
|
new_position(i) = params.gui.new_theta[i];
|
|
ArmControl::degreesToRadians(new_position);
|
|
ctl.moveToPosition(new_position);
|
|
update_gui_position(ctl, params);
|
|
break;
|
|
|
|
case DRAW:
|
|
params.new_mode = NONE;
|
|
printf("Drawing\n");
|
|
if (params.ctl.holding_marker) {
|
|
//ctl.drawX();
|
|
} else {
|
|
params.new_mode = ERROR;
|
|
params.error = "Must hold marker to draw.";
|
|
}
|
|
break;
|
|
|
|
// end movement modes
|
|
case GRAB:
|
|
params.new_mode = NONE;
|
|
printf("Grab marker\n");
|
|
if (!params.ctl.holding_marker) {
|
|
ctl.grabMarker();
|
|
//sleep(1);
|
|
params.ctl.holding_marker = true;
|
|
} else {
|
|
printf("error set\n");
|
|
params.error_set = true;
|
|
params.error = "Marker already held\n";
|
|
}
|
|
break;
|
|
|
|
case RELEASE:
|
|
params.new_mode = NONE;
|
|
printf("Release marker\n");
|
|
if (params.ctl.holding_marker) {
|
|
ctl.openGrip();
|
|
params.ctl.holding_marker = false;
|
|
} else {
|
|
params.error_set = true;
|
|
params.error = "Marker not being held\n";
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
// update param struct
|
|
|
|
continue; //to next mainloop iteration
|
|
|
|
} //end of kinematics
|
|
|
|
//
|
|
// Setup code for Image Processing and Visual Servoing
|
|
//
|
|
|
|
if (capture == 0) {
|
|
|
|
capture = cvCreateCameraCapture(cameraID);
|
|
|
|
if (capture == 0) {
|
|
printf("failed to init capture device\n");
|
|
sleep(1); continue; //to next mainloop iteration
|
|
}
|
|
|
|
printf("initialized capture device\n");
|
|
printf("allocating images\n");
|
|
|
|
images.bgr = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 3);
|
|
|
|
#if FLOAT_HLS
|
|
images.bgr32 = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_32F, 3);
|
|
images.hls = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_32F, 3);
|
|
#else
|
|
images.hls = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 3);
|
|
#endif
|
|
|
|
images.filtered_bgr = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 1);
|
|
images.filtered_hls = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 1);
|
|
|
|
if (images.bgr == 0 || images.hls == 0
|
|
#if FLOAT_HLS
|
|
|| images.bgr32 == 0
|
|
#endif
|
|
|| images.filtered_bgr == 0 || images.filtered_hls == 0) {
|
|
params.current_mode = ERROR;
|
|
params.error = "Cannot create image holders";
|
|
std::cout << params.error << std::endl;
|
|
params.run = false;
|
|
continue; //to next mainloop iteration
|
|
}
|
|
|
|
//some images might be displayed before being initialized
|
|
|
|
cvSet(images.bgr, cvScalar(0,0,0));
|
|
#if FLOAT_HLS
|
|
cvSet(images.bgr32, cvScalar(0,0,0));
|
|
#endif
|
|
cvSet(images.hls, cvScalar(0,0,0));
|
|
cvSet(images.filtered_bgr, cvScalar(0));
|
|
cvSet(images.filtered_hls, cvScalar(0));
|
|
|
|
images.set = true;
|
|
|
|
} //capture was 0
|
|
|
|
//
|
|
// Image Processing
|
|
//
|
|
frame = cvQueryFrame(capture);
|
|
|
|
if (frame == 0) {
|
|
params.current_mode = ERROR;
|
|
params.error = "Null frame";
|
|
std::cout << params.error << std::endl;
|
|
params.run = false;
|
|
continue; //to next mainloop iteration
|
|
}
|
|
|
|
cvResize(frame, images.bgr);
|
|
|
|
ctl.slaveWrist(true);
|
|
|
|
ip.filterImages(images, params);
|
|
|
|
if (!params.gui.target_set) continue;
|
|
|
|
if (params.activity == VS ||
|
|
params.activity == IP) {
|
|
//find ball
|
|
|
|
circles = ip.findBall(images, params);
|
|
|
|
mark_images(target_pos, circles, params, images);
|
|
|
|
} //find ball
|
|
|
|
if (params.activity != VS) {
|
|
usleep(1000);
|
|
continue; //to next mainloop iteration
|
|
}
|
|
|
|
//
|
|
// Visual Servoing code
|
|
//
|
|
|
|
switch (params.new_mode) {
|
|
case GRAB: params.new_mode = NONE; ctl.grabBall(); break;
|
|
case RELEASE: params.new_mode = NONE; ctl.openGrip(); break;
|
|
default: break;
|
|
}
|
|
|
|
printf("current_mode = %d\n", params.current_mode);
|
|
switch (params.current_mode) {
|
|
case READY:
|
|
printf("old: READY\t");
|
|
switch (params.new_mode) {
|
|
case MOVE:
|
|
printf("new: MOVE\n");
|
|
params.new_mode = NONE;
|
|
params.current_mode = MOVE;
|
|
break;
|
|
case PAUSE:
|
|
printf("new: PAUSE\n");
|
|
params.new_mode = NONE;
|
|
params.current_mode = PAUSE;
|
|
continue; //to next mainloop iteration
|
|
default:
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case PAUSE:
|
|
printf("old: PAUSE\t");
|
|
if (params.new_mode == MOVE) {
|
|
printf("new: MOVE\n");
|
|
params.new_mode = NONE;
|
|
params.current_mode = MOVE;
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case MOVE:
|
|
printf("old: MOVE\t");
|
|
if (params.new_mode == PAUSE) {
|
|
printf("new: PAUSE\n");
|
|
params.new_mode = NONE;
|
|
//ctl.stop();
|
|
params.current_mode = PAUSE;
|
|
continue; //to next mainloop iteration
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (circles != 0 && circles->total > 0 &&
|
|
params.gui.target_set &&
|
|
(params.current_mode == MOVE || params.current_mode == GRAB)) {
|
|
ublas::vector<double> features(3);
|
|
|
|
float* p = (float*) cvGetSeqElem(circles, 0);
|
|
|
|
printf("first circle at (%d,%d) radius %d\n",
|
|
cvRound(p[0]), cvRound(p[1]), cvRound(p[2]));
|
|
|
|
features(0) = p[0]; features(1) = p[1]; features(2) = p[2];
|
|
|
|
if (features(2) >= MIN_TRACKING_RADIUS_PIXELS) {
|
|
|
|
// rotate/translate to center origin, x left, y up
|
|
features(0) = (images.hls->width / 2.0) - features(0); // x
|
|
if (images.hls->origin == 0) // top left origin
|
|
features(1) = (images.hls->height / 2.0) - features(1); // y
|
|
|
|
// normalize x & y
|
|
features(0) /= FOCAL_LENGTH; features(1) /= FOCAL_LENGTH;
|
|
|
|
// circular approximation of Z
|
|
// Z = D*f / radius*2
|
|
features(2) = DIAMETER * FOCAL_LENGTH / (features(2) * 2.0);
|
|
|
|
printf("Norm features x,y = (%3f, %3f), Z = %3f\n",
|
|
features(0), features(1), features(2));
|
|
printf("Norm target x,y = (%3f, %3f), Z = %3f\n",
|
|
target_pos_norm(0), target_pos_norm(1), target_pos_norm(2));
|
|
|
|
std::cout << "current angles: " << ctl.getCurrentAngles() << std::endl;
|
|
|
|
bool dls = ik.damped_least_squares(features, ctl.getCurrentAngles(),
|
|
params, delta_angles);
|
|
|
|
if (dls && params.current_mode != PARK) {
|
|
std::cout << "commanded angle deltas: " << delta_angles << std::endl;
|
|
ctl.moveDelta(delta_angles);
|
|
}
|
|
|
|
} else {
|
|
std::cout <<
|
|
"radius below tracking enable threshold " <<
|
|
MIN_TRACKING_RADIUS_PIXELS;
|
|
}
|
|
} //tracking ball
|
|
|
|
} //mainloop
|
|
|
|
#if RUN_THREADED
|
|
switch (pthread_join(guiTID, 0)) {
|
|
case 0: break; // all ok
|
|
case EINVAL:
|
|
printf("pthread_join: Invalid thread id %d\n", (int) guiTID); break;
|
|
case ESRCH:
|
|
printf("pthread_join: Thread ID %d not found\n", (int) guiTID); break;
|
|
case EDEADLK:
|
|
printf("pthread_join: Deadlock detected\n"); break;
|
|
default:
|
|
break;
|
|
}
|
|
#endif
|
|
|
|
if (images.set) {
|
|
printf("releasing images\n");
|
|
cvReleaseImage(&(images.bgr));
|
|
cvReleaseImage(&(images.hls));
|
|
cvReleaseImage(&(images.filtered_hls));
|
|
cvReleaseImage(&(images.filtered_bgr));
|
|
#ifdef FLOAT_HLS
|
|
cvReleaseImage(&(images.bgr32));
|
|
#endif
|
|
}
|
|
|
|
if (gui != 0) {
|
|
printf("destroying gui\n");
|
|
gui->destroy();
|
|
gui = 0;
|
|
}
|
|
|
|
if (capture != 0) {
|
|
printf("releasing capture device\n");
|
|
cvReleaseCapture(&capture);
|
|
}
|
|
|
|
} //main()
|
|
|
|
|
|
/*!
|
|
* \brief Markup images with circles and lines.
|
|
* Used for giving feedback to the user on the location of the visual
|
|
* servo target and where the ball is detected in the image.
|
|
*
|
|
* \param[in] target Cartesian coordinates of the target in [pixel,
|
|
* pixel, meter] units.
|
|
* \param[in] circles Sequence of detected circles (u,v,r) in pixels
|
|
* \param[in] params Params struct
|
|
* \param[in,out] images Images struct
|
|
*/
|
|
void mark_images(const ublas::vector<double>& target, const CvSeq* circles,
|
|
const Params& params, Images& images) {
|
|
|
|
// draw target cross
|
|
if (params.gui.target_set && params.activity == VS) {
|
|
// fl * D / Z = apparent diameter, so div by 2 to get apparent radius
|
|
double radius = (FOCAL_LENGTH * DIAMETER / target(2)) / 2.0;
|
|
|
|
// rescale since target(x,y) was normalized using FOCAL_LENGTH
|
|
double ih = images.bgr->height/2.0;
|
|
double iw = images.bgr->width/2.0;
|
|
CvPoint v1 = cvPoint(cvRound(target(0) + iw ),
|
|
cvRound(target(1) + ih - radius)); // up
|
|
CvPoint v2 = cvPoint(cvRound(target(0) + iw ),
|
|
cvRound(target(1) + ih + radius)); // down
|
|
CvPoint h1 = cvPoint(cvRound(target(0) + iw - radius),
|
|
cvRound(target(1) + ih )); // left
|
|
CvPoint h2 = cvPoint(cvRound(target(0) + iw + radius),
|
|
cvRound(target(1) + ih )); // right
|
|
|
|
// Draw target cross for sighting.
|
|
cvLine(images.bgr, h1, h2, CV_RGB(0x00, 0x00, 0xff));
|
|
cvLine(images.bgr, v1, v2, CV_RGB(0x00, 0x00, 0xff));
|
|
}
|
|
|
|
int num_circles = /*params.activity == VS ? 1 :*/ circles->total;
|
|
|
|
// draw the ball
|
|
for (int i = 0; i < num_circles; i++ ) {
|
|
float* p = (float*) cvGetSeqElem(circles, i);
|
|
CvPoint pt = cvPoint(cvRound(p[0]),cvRound(p[1]));
|
|
cvCircle(images.bgr, pt, cvRound(p[2]), CV_RGB(0xff, 0x00, 0x00));
|
|
cvCircle(images.filtered_hls, pt, cvRound(p[2]), cvScalar(192)); //greyscale
|
|
//TBD mark filtered_bgr if using that to find the ball
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* \brief Uses calibration file to set offsets.
|
|
* Reads servo numbers and calibration positions from the provided
|
|
* file. Offsets are calculated from calibration position differences
|
|
* to ideal positions.
|
|
*/
|
|
void
|
|
calibrate_offsets(std::string& file, ublas::vector<double>& offsets){
|
|
if (file.empty()) {
|
|
offsets(Arm::ELBOW) = 400;
|
|
} else {
|
|
std::fstream input(file.c_str());
|
|
int servo, val;
|
|
ublas::vector<double> calibration_position(NUM_JOINTS);
|
|
calibration_position(Arm::GRIP) = 1350;
|
|
calibration_position(Arm::WRIST) = 1500;
|
|
calibration_position(Arm::ELBOW) = 1500;
|
|
calibration_position(Arm::SHOULDER) = 1500;
|
|
calibration_position(Arm::BASE) = 1500;
|
|
std::cout << "cal: " << calibration_position << std::endl;
|
|
std::cout << "grip: " << Arm::GRIP << std::endl;
|
|
while (!input.eof()) {
|
|
input >> std::skipws >> servo >> val;
|
|
printf("servo: %d, val: %d, cal: %g\t",
|
|
servo, val, calibration_position(servo));
|
|
offsets[servo] = val - calibration_position(servo);
|
|
printf("offset: %g\n", offsets(servo));
|
|
}
|
|
std::cout << "off: " << offsets << std::endl;
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* \brief Update params with current angles.
|
|
* Sets current angles of ctl in struct params to be picked up by GUI.
|
|
*/
|
|
void
|
|
update_gui_position (ArmControl& ctl, Params& params) {
|
|
ublas::vector<double> current_theta(ctl.getCurrentAngles());
|
|
ArmControl::radiansToDegrees(current_theta);
|
|
for (int i = 0; i < NUM_JOINTS; i++) {
|
|
params.ctl.current_theta[i] = current_theta(i);
|
|
}
|
|
params.position_changed = true;
|
|
}
|
|
|
|
/*!
|
|
* \brief Signal handler function for stopping threads.
|
|
*
|
|
* \param[in] sig The received signal.
|
|
*/
|
|
void handler(int sig) {
|
|
if (sig_ctl == 0) {
|
|
printf("No control object for emergency shutdown!\n");
|
|
} else {
|
|
sig_ctl->stop();
|
|
}
|
|
exit(sig);
|
|
}
|