diff --git a/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro b/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro index 6aa20ac..f6dd516 100644 --- a/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro +++ b/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro @@ -33,7 +33,8 @@ SOURCES += \ matrix/transformation.cpp \ opengl/oglObjects.cpp \ serialport/serialport.cpp \ - dynamixel/dynamixel.cpp + dynamixel/dynamixel.cpp \ + interpolation.cpp HEADERS += \ mainwindow.h \ @@ -48,7 +49,8 @@ HEADERS += \ opengl/oglDef.h \ opengl/oglObjects.h \ serialport/serialport.h \ - dynamixel/dynamixel.h + dynamixel/dynamixel.h \ + interpolation.h FORMS += \ mainwindow.ui diff --git a/PhantomX-Robot-Arm-Kit/glwidget.cpp b/PhantomX-Robot-Arm-Kit/glwidget.cpp index 728f1b9..c5e3834 100644 --- a/PhantomX-Robot-Arm-Kit/glwidget.cpp +++ b/PhantomX-Robot-Arm-Kit/glwidget.cpp @@ -8,6 +8,9 @@ GLWidget::GLWidget(QWidget *parent) : _angleHor = -30.0; _angleVer = 10.0; _fovAngle = 45.0; + + _node = NULL; + _path = NULL; } void GLWidget::initializeGL() { @@ -51,6 +54,15 @@ void GLWidget::paintGL() { setViewport(); oglPlane(5.0, 0.5); + if(_node) { + glColor3d(0.5, 0.0, 0.5); + renderPath (_node); + } + if(_path) { + glColor3d(1.0, 0.0, 1.0); + renderPath (_path); + } + renderTarget(); renderJoint(); @@ -90,7 +102,6 @@ void GLWidget::mouseMoveEvent(QMouseEvent *event) { void GLWidget::mousePressEvent(QMouseEvent *event) { - if(event->buttons() != Qt::MiddleButton) { _mouseDownPoint = event->localPos(); } else { @@ -124,8 +135,15 @@ void GLWidget::setKinematics(CKinematics *kinematics) { _kinematics = kinematics; } -void GLWidget::setJointAngle(dVector &q) -{ +void GLWidget::setPath(std::vector> *path) { + _path = path; +} + +void GLWidget::setNode(std::vector> *node) { + _node = node; +} + +void GLWidget::setJointAngle(dVector &q) { _kinematics->SetJointAngle(q); } @@ -149,7 +167,17 @@ void GLWidget::transformAxis(dMatrix A) { glMultMatrixd(m); } -void GLWidget::renderTarget () { +void GLWidget::renderPath(std::vector> *path) +{ + std::vector line; + for (unsigned int i=0; isize(); ++i) { + std::valarray &p = (*path)[i]; + line.push_back (CPoint3d(p[1], p[2], p[3])); + } + oglLineStrip (line); +} + +void GLWidget::renderTarget() { dVector desired = _kinematics->GetDesired (); glPushMatrix(); diff --git a/PhantomX-Robot-Arm-Kit/glwidget.h b/PhantomX-Robot-Arm-Kit/glwidget.h index 71269fd..b4eb886 100644 --- a/PhantomX-Robot-Arm-Kit/glwidget.h +++ b/PhantomX-Robot-Arm-Kit/glwidget.h @@ -27,6 +27,8 @@ class GLWidget : public QGLWidget public: explicit GLWidget(QWidget *parent = 0); + void setPath(std::vector> *path); + void setNode(std::vector> *node); void setKinematics(CKinematics *kinematics); void setJointAngle(dVector &q); @@ -58,17 +60,20 @@ class GLWidget : public QGLWidget // kinematics CKinematics *_kinematics; + std::vector> *_node; + std::vector> *_path; void setViewport(); void transformAxis(dMatrix A); - void drawRevLink (double x, double y, double z, double radius); + void drawRevLink(double x, double y, double z, double radius); - void drawFixedJoint (JointInfo *joint); - void drawRevoluteJoint (JointInfo *joint); - void drawPrismaticJoint (JointInfo *joint); + void drawFixedJoint(JointInfo *joint); + void drawRevoluteJoint(JointInfo *joint); + void drawPrismaticJoint(JointInfo *joint); - void renderTarget (); + void renderPath(std::vector> *path); + void renderTarget(); void renderJoint(); }; diff --git a/PhantomX-Robot-Arm-Kit/interpolation.cpp b/PhantomX-Robot-Arm-Kit/interpolation.cpp new file mode 100644 index 0000000..25b3c20 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/interpolation.cpp @@ -0,0 +1,31 @@ +#include "interpolation.h" + +std::vector> linearInterpolation (std::vector> &x, double timeSlice) { + if (timeSlice < 0.001) timeSlice = 0.001; + if (x.size() < 2) return x; + + unsigned int n = x.size () - 1; + + std::vector h(n); + for (unsigned int i=0; i> s; + s.reserve ((int)(x[n][0] / timeSlice + 10)); + + double t = x[0][0]; + for (unsigned int i=0; i m = (x[i+1] - x[i])/h[i]; + + for (; t<=x[i+1][0]; t+=timeSlice) { + std::valarray p = m*(t - x[i][0]) + x[i]; + p[0] = t; + s.push_back (p); + } + } + s.push_back (x[n]); + + return s; +} diff --git a/PhantomX-Robot-Arm-Kit/interpolation.h b/PhantomX-Robot-Arm-Kit/interpolation.h new file mode 100644 index 0000000..fe2c830 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/interpolation.h @@ -0,0 +1,11 @@ +#ifndef INTERPOLATION_H +#define INTERPOLATION_H + +#include +#include + +#include + +std::vector> linearInterpolation (std::vector> &x, double timeSlice); + +#endif // INTERPOLATION_H diff --git a/PhantomX-Robot-Arm-Kit/main.cpp b/PhantomX-Robot-Arm-Kit/main.cpp index 818dc8d..e88f7a9 100644 --- a/PhantomX-Robot-Arm-Kit/main.cpp +++ b/PhantomX-Robot-Arm-Kit/main.cpp @@ -1,7 +1,7 @@ #include "mainwindow.h" #include -#define APP_VERSION "1.2.0" +#define APP_VERSION "1.3.0" int main(int argc, char *argv[]) { diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index a51a8d3..373925b 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -5,86 +5,7 @@ MainWindow::MainWindow(QWidget *parent) :QMainWindow(parent), ui(new Ui::MainWin ui->setupUi(this); - // set ui object - _sliderForward = new QSlider*[5]; - _sliderForward[0] = ui->horizontalSlider0; - _sliderForward[1] = ui->horizontalSlider1; - _sliderForward[2] = ui->horizontalSlider2; - _sliderForward[3] = ui->horizontalSlider3; - _sliderForward[4] = ui->horizontalSlider4; - - _lineEditForward = new QLineEdit*[5]; - _lineEditForward[0] = ui->lineEdit0; - _lineEditForward[1] = ui->lineEdit1; - _lineEditForward[2] = ui->lineEdit2; - _lineEditForward[3] = ui->lineEdit3; - _lineEditForward[4] = ui->lineEdit4; - - _labelForward = new QLabel*[6]; - _labelForward[0] = ui->labelForwardX; - _labelForward[1] = ui->labelForwardY; - _labelForward[2] = ui->labelForwardZ; - _labelForward[3] = ui->labelForwardPhi; - _labelForward[4] = ui->labelForwardTheta; - _labelForward[5] = ui->labelForwardPsi; - - for(int i=0; i<5; i++) { - - // set range, only number - _lineEditForward[i]->setValidator( new QIntValidator(this) ); - - // set text signal - connect(_lineEditForward[i], &QLineEdit::textChanged, [=](QString text) { - bool ok = false; - _sliderForward[i]->setValue(text.toInt(&ok)); - }); - - // set slider signal - connect(_sliderForward[i], &QSlider::valueChanged, [=](int value) { forwardValueChanged(i,value); }); - } - - // set ui object - _sliderInverse = new QSlider*[6]; - _sliderInverse[0] = ui->horizontalSliderX; - _sliderInverse[1] = ui->horizontalSliderY; - _sliderInverse[2] = ui->horizontalSliderZ; - _sliderInverse[3] = ui->horizontalSliderPhi; - _sliderInverse[4] = ui->horizontalSliderTheta; - _sliderInverse[5] = ui->horizontalSliderPsi; - - _lineEditInverse = new QLineEdit*[6]; - _lineEditInverse[0] = ui->lineEditX; - _lineEditInverse[1] = ui->lineEditY; - _lineEditInverse[2] = ui->lineEditZ; - _lineEditInverse[3] = ui->lineEditPhi; - _lineEditInverse[4] = ui->lineEditTheta; - _lineEditInverse[5] = ui->lineEditPsi; - - _labelInverse = new QLabel*[5]; - _labelInverse[0] = ui->labelInverseQ0; - _labelInverse[1] = ui->labelInverseQ1; - _labelInverse[2] = ui->labelInverseQ2; - _labelInverse[3] = ui->labelInverseQ3; - _labelInverse[4] = ui->labelInverseQ4; - - for(int i=0; i<6; i++) { - - // set range, only number - _lineEditInverse[i]->setValidator( new QIntValidator(this) ); - - // set text signal - connect(_lineEditInverse[i], &QLineEdit::textChanged, [=](QString text) { - bool ok = false; - _sliderInverse[i]->setValue(text.toInt(&ok)); - }); - - // set slider signal - connect(_sliderInverse[i], &QSlider::valueChanged, [=](int value) { inverseValueChanged(i,value); }); - } - - - - + setUiObject(); // set link, joint _kinematics = new CPosOriInverse(POSITION_ORIENTATION); @@ -153,30 +74,102 @@ MainWindow::~MainWindow() { delete ui; } -void MainWindow::printForwardKinematics() { +void MainWindow::doUserTask() { - dVector value = CTransformMatrix(_kinematics->Forward()).GetPositionOrientation(); - QString text; + if(ui->tabWidget->currentIndex() == FORWARD) { + dVector value = CTransformMatrix(_kinematics->Forward()).GetPositionOrientation(); + _kinematics->SetDesired(value[0], value[1], value[2], value[3], value[4], value[5]); - for(int i=0;i<6;i++) { - if(i < 3) { - text.sprintf("%4.0f",value[i] * 1000.0); - } else { - text.sprintf("%4.0f",value[i] * _RAD2DEG); + for(int i=0; i<6; i++) { + if(i < 3) { + _sliderInverse[i]->setValue(value[i] * 1000.0); + } else { + _sliderInverse[i]->setValue(value[i] * _RAD2DEG); + } } - _labelForward[i]->setText(text); + printForwardKinematics(); + ui->widget->setNode(NULL); + ui->widget->setPath(NULL); + _interpolation.node.clear(); + _interpolation.path.clear(); + } else if(ui->tabWidget->currentIndex() == INVERSE) { + _kinematics->SetDesired(_target[0], _target[1], _target[2], _target[3], _target[4], _target[5]); + _q.target += _kinematics->SolveDLS(0.01, 0.03, 0.01); + + for(int i=0; i<5; i++) { + _sliderForward[i]->setValue(_q.target[i] * _RAD2DEG); + } + + printInverseKinematics(); + ui->widget->setNode(NULL); + ui->widget->setPath(NULL); + _interpolation.node.clear(); + _interpolation.path.clear(); + } else if(ui->tabWidget->currentIndex() == PATH) { + if(_interpolation.path.size()) { + if(_interpolation.pathCount >= _interpolation.path.size()) { + _interpolation.pathCount = 0; + } + + for(int i=0; i<6; i++) { + _target[i] = _interpolation.path[_interpolation.pathCount][i+1]; + } + + _kinematics->SetDesired(_target[0], _target[1], _target[2], _target[3], _target[4], _target[5]); + _q.target += _kinematics->SolveDLS(0.01, 0.03, 0.01); + + for(int i=0; i<5; i++) { + _sliderForward[i]->setValue(_q.target[i] * _RAD2DEG); + } + + for(int i=0; i<6; i++) { + if(i < 3) { + _sliderInverse[i]->setValue(_target[i] * 1000.0); + } else { + _sliderInverse[i]->setValue(_target[i] * _RAD2DEG); + } + } + + printForwardKinematics(); + printInverseKinematics(); + _interpolation.pathCount++; + } } -} -void MainWindow::printInverseKinematics() { + if(_serialPort->isOpen()) { + _serialPort->write(_dynamixel->generateJointAnglePacket(_q.target)); + } + ui->widget->setJointAngle(_q.target); + _kinematics->Forward(); + ui->widget->updateGL(); - dVector value = _kinematics->GetJointAngle(); - QString text; + if(!_messageQueue->isEmpty()) { + QByteArray buffer = _messageQueue->dequeue(); + int16_t id = buffer[2]; + uint16_t pos = (((uint16_t)buffer[6] << 8) & 0xFF00) | (buffer[5] & 0xFF); - for(int i=0;i<5;i++) { - text.sprintf("%4.0f",value[i] * _RAD2DEG); - _labelInverse[i]->setText(text); + switch(id) { + case 1: + _q.current[0] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; + break; + + case 3: + _q.current[1] = (_dynamixel->convertDynamixelToAngle(pos) - Dynamixel::offset::LINK + Dynamixel::offset::ANGLE) * _DEG2RAD; + break; + + case 4: + _q.current[2] = (_dynamixel->convertDynamixelToAngle(pos) + Dynamixel::offset::LINK - Dynamixel::offset::ANGLE) * _DEG2RAD; + break; + + case 6: + _q.current[3] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; + break; + + case 7: + _q.current[4] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; + break; + } } } @@ -205,16 +198,12 @@ dVector MainWindow::getInverseSliderValue() { } void MainWindow::forwardValueChanged(int index, int val) { - _lineEditForward[index]->setText(QString::number(val)); - _q.target[index] = val * _DEG2RAD; } void MainWindow::inverseValueChanged(int index, int val) { - _lineEditInverse[index]->setText(QString::number(val)); - if(index < 3) { _target[index] = val / 1000.0; } else { @@ -225,84 +214,221 @@ void MainWindow::inverseValueChanged(int index, int val) { void MainWindow::on_buttonReset_clicked() { for(int i=0; i<5;i++) { - _sliderForward[i]->setValue(_q.init[i] * _RAD2DEG); + _lineEditForward[i]->setText(QString::number(_q.init[i] * _RAD2DEG)); } - ui->widget->setJointAngle(_q.init); - ui->widget->updateGL(); dVector value = CTransformMatrix(_kinematics->Forward()).GetPositionOrientation(); - for(int i=0; i<6; i++) { if(i < 3) { - _sliderInverse[i]->setValue(value[i] * 1000.0); + _lineEditInverse[i]->setText(QString::number(value[i] * 1000.0)); } else { - _sliderInverse[i]->setValue(value[i] * _RAD2DEG); + _lineEditInverse[i]->setText(QString::number(value[i] * _RAD2DEG)); } } + _target = value; + _kinematics->SetDesired(_target[0], _target[1], _target[2], _target[3], _target[4], _target[5]); + ui->widget->updateGL(); + + _interpolation.pathCount = 0; + _interpolation.node.clear(); + _interpolation.path.clear(); + + for(int i=0; i<8; i++) { + _lineEditPath[i][0]->setText(QString::number(_interpolation.init[i][0])); + _lineEditPath[i][1]->setText(QString::number(_interpolation.init[i][1] * 1000.0)); + _lineEditPath[i][2]->setText(QString::number(_interpolation.init[i][2] * 1000.0)); + _lineEditPath[i][3]->setText(QString::number(_interpolation.init[i][3] * 1000.0)); + _lineEditPath[i][4]->setText(QString::number(_interpolation.init[i][4] * _RAD2DEG)); + _lineEditPath[i][5]->setText(QString::number(_interpolation.init[i][5] * _RAD2DEG)); + _lineEditPath[i][6]->setText(QString::number(_interpolation.init[i][6] * _RAD2DEG)); + } } -void MainWindow::doUserTask() { +void MainWindow::on_buttonPathApply_clicked() { + bool ok = false; + + _interpolation.pathCount = 0; + _interpolation.node.clear(); + _interpolation.path.clear(); + + for(int i=0; i<8; i++) { + double a[7] = {0.0, }; + a[0] = _lineEditPath[i][0]->text().toDouble(&ok); + a[1] = _lineEditPath[i][1]->text().toDouble(&ok) / 1000.0; + a[2] = _lineEditPath[i][2]->text().toDouble(&ok) / 1000.0; + a[3] = _lineEditPath[i][3]->text().toDouble(&ok) / 1000.0; + a[4] = _lineEditPath[i][4]->text().toDouble(&ok) * _DEG2RAD; + a[5] = _lineEditPath[i][5]->text().toDouble(&ok) * _DEG2RAD; + a[6] = _lineEditPath[i][6]->text().toDouble(&ok) * _DEG2RAD; + _interpolation.node.push_back (std::valarray(&a[0], 7)); + } - if(ui->tabWidget->currentIndex() == FORWARD) { - dVector value = CTransformMatrix(_kinematics->Forward()).GetPositionOrientation(); + _interpolation.path = linearInterpolation(_interpolation.node, (double)TASK_TIME/1000.0); - for(int i=0; i<6; i++) { - if(i < 3) { - _sliderInverse[i]->setValue(value[i] * 1000.0); - } else { - _sliderInverse[i]->setValue(value[i] * _RAD2DEG); - } - } + ui->widget->setNode(&_interpolation.node); + ui->widget->setPath(&_interpolation.path); +} - _kinematics->SetDesired(value[0], value[1], value[2], value[3], value[4], value[5]); - printForwardKinematics(); - } else if(ui->tabWidget->currentIndex() == INVERSE) { - _kinematics->SetDesired(_target[0], _target[1], _target[2], _target[3], _target[4], _target[5]); - dVector dq = _kinematics->SolveDLS(0.01, 0.03, 0.01); - _q.target += dq; +void MainWindow::setUiObject() { + // forward + _sliderForward = new QSlider*[5]; + _sliderForward[0] = ui->horizontalSlider0; + _sliderForward[1] = ui->horizontalSlider1; + _sliderForward[2] = ui->horizontalSlider2; + _sliderForward[3] = ui->horizontalSlider3; + _sliderForward[4] = ui->horizontalSlider4; - for(int i=0; i<5; i++) { - _sliderForward[i]->setValue(_q.target[i] * _RAD2DEG); - } + _lineEditForward = new QLineEdit*[5]; + _lineEditForward[0] = ui->lineEdit0; + _lineEditForward[1] = ui->lineEdit1; + _lineEditForward[2] = ui->lineEdit2; + _lineEditForward[3] = ui->lineEdit3; + _lineEditForward[4] = ui->lineEdit4; - printInverseKinematics(); - } + _labelForward = new QLabel*[6]; + _labelForward[0] = ui->labelForwardX; + _labelForward[1] = ui->labelForwardY; + _labelForward[2] = ui->labelForwardZ; + _labelForward[3] = ui->labelForwardPhi; + _labelForward[4] = ui->labelForwardTheta; + _labelForward[5] = ui->labelForwardPsi; - if(_serialPort->isOpen()) { - _serialPort->write(_dynamixel->generateJointAnglePacket(_q.target)); + for(int i=0; i<5; i++) { + + // set range, only number + _lineEditForward[i]->setValidator( new QIntValidator(this) ); + + // set text signal + connect(_lineEditForward[i], &QLineEdit::textChanged, [=](QString text) { + bool ok = false; + _sliderForward[i]->setValue(text.toInt(&ok)); + }); + + // set slider signal + connect(_sliderForward[i], &QSlider::valueChanged, [=](int value) { forwardValueChanged(i,value); }); } - ui->widget->setJointAngle(_q.target); - _kinematics->Forward(); - ui->widget->updateGL(); + // set inverse + _sliderInverse = new QSlider*[6]; + _sliderInverse[0] = ui->horizontalSliderX; + _sliderInverse[1] = ui->horizontalSliderY; + _sliderInverse[2] = ui->horizontalSliderZ; + _sliderInverse[3] = ui->horizontalSliderPhi; + _sliderInverse[4] = ui->horizontalSliderTheta; + _sliderInverse[5] = ui->horizontalSliderPsi; + + _lineEditInverse = new QLineEdit*[6]; + _lineEditInverse[0] = ui->lineEditX; + _lineEditInverse[1] = ui->lineEditY; + _lineEditInverse[2] = ui->lineEditZ; + _lineEditInverse[3] = ui->lineEditPhi; + _lineEditInverse[4] = ui->lineEditTheta; + _lineEditInverse[5] = ui->lineEditPsi; - if(!_messageQueue->isEmpty()) { - QByteArray buffer = _messageQueue->dequeue(); - int16_t id = buffer[2]; - uint16_t pos = (((uint16_t)buffer[6] << 8) & 0xFF00) | (buffer[5] & 0xFF); + _labelInverse = new QLabel*[5]; + _labelInverse[0] = ui->labelInverseQ0; + _labelInverse[1] = ui->labelInverseQ1; + _labelInverse[2] = ui->labelInverseQ2; + _labelInverse[3] = ui->labelInverseQ3; + _labelInverse[4] = ui->labelInverseQ4; - switch(id) { - case 1: - _q.current[0] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; - break; + for(int i=0; i<6; i++) { - case 3: - _q.current[1] = (_dynamixel->convertDynamixelToAngle(pos) - Dynamixel::offset::LINK + Dynamixel::offset::ANGLE) * _DEG2RAD; - break; + // set range, only number + _lineEditInverse[i]->setValidator( new QIntValidator(this) ); - case 4: - _q.current[2] = (_dynamixel->convertDynamixelToAngle(pos) + Dynamixel::offset::LINK - Dynamixel::offset::ANGLE) * _DEG2RAD; - break; + // set text signal + connect(_lineEditInverse[i], &QLineEdit::textChanged, [=](QString text) { + bool ok = false; + _sliderInverse[i]->setValue(text.toInt(&ok)); + }); - case 6: - _q.current[3] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; - break; + // set slider signal + connect(_sliderInverse[i], &QSlider::valueChanged, [=](int value) { inverseValueChanged(i,value); }); + } - case 7: - _q.current[4] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; - break; + // set path + _lineEditPath = new QLineEdit**[8]; + for(int i=0; i<8; i++) { + _lineEditPath[i] = new QLineEdit*[7]; + } + + _lineEditPath[0][0] = ui->lineEditPathTime0; + _lineEditPath[1][0] = ui->lineEditPathTime1; + _lineEditPath[2][0] = ui->lineEditPathTime2; + _lineEditPath[3][0] = ui->lineEditPathTime3; + _lineEditPath[4][0] = ui->lineEditPathTime4; + _lineEditPath[5][0] = ui->lineEditPathTime5; + _lineEditPath[6][0] = ui->lineEditPathTime6; + _lineEditPath[7][0] = ui->lineEditPathTime7; + + _lineEditPath[0][1] = ui->lineEditPathX0; _lineEditPath[0][2] = ui->lineEditPathY0; _lineEditPath[0][3] = ui->lineEditPathZ0; + _lineEditPath[1][1] = ui->lineEditPathX1; _lineEditPath[1][2] = ui->lineEditPathY1; _lineEditPath[1][3] = ui->lineEditPathZ1; + _lineEditPath[2][1] = ui->lineEditPathX2; _lineEditPath[2][2] = ui->lineEditPathY2; _lineEditPath[2][3] = ui->lineEditPathZ2; + _lineEditPath[3][1] = ui->lineEditPathX3; _lineEditPath[3][2] = ui->lineEditPathY3; _lineEditPath[3][3] = ui->lineEditPathZ3; + _lineEditPath[4][1] = ui->lineEditPathX4; _lineEditPath[4][2] = ui->lineEditPathY4; _lineEditPath[4][3] = ui->lineEditPathZ4; + _lineEditPath[5][1] = ui->lineEditPathX5; _lineEditPath[5][2] = ui->lineEditPathY5; _lineEditPath[5][3] = ui->lineEditPathZ5; + _lineEditPath[6][1] = ui->lineEditPathX6; _lineEditPath[6][2] = ui->lineEditPathY6; _lineEditPath[6][3] = ui->lineEditPathZ6; + _lineEditPath[7][1] = ui->lineEditPathX7; _lineEditPath[7][2] = ui->lineEditPathY7; _lineEditPath[7][3] = ui->lineEditPathZ7; + + _lineEditPath[0][4] = ui->lineEditPathPhi0; _lineEditPath[0][5] = ui->lineEditPathTheta0; _lineEditPath[0][6] = ui->lineEditPathPsi0; + _lineEditPath[1][4] = ui->lineEditPathPhi1; _lineEditPath[1][5] = ui->lineEditPathTheta1; _lineEditPath[1][6] = ui->lineEditPathPsi1; + _lineEditPath[2][4] = ui->lineEditPathPhi2; _lineEditPath[2][5] = ui->lineEditPathTheta2; _lineEditPath[2][6] = ui->lineEditPathPsi2; + _lineEditPath[3][4] = ui->lineEditPathPhi3; _lineEditPath[3][5] = ui->lineEditPathTheta3; _lineEditPath[3][6] = ui->lineEditPathPsi3; + _lineEditPath[4][4] = ui->lineEditPathPhi4; _lineEditPath[4][5] = ui->lineEditPathTheta4; _lineEditPath[4][6] = ui->lineEditPathPsi4; + _lineEditPath[5][4] = ui->lineEditPathPhi5; _lineEditPath[5][5] = ui->lineEditPathTheta5; _lineEditPath[5][6] = ui->lineEditPathPsi5; + _lineEditPath[6][4] = ui->lineEditPathPhi6; _lineEditPath[6][5] = ui->lineEditPathTheta6; _lineEditPath[6][6] = ui->lineEditPathPsi6; + _lineEditPath[7][4] = ui->lineEditPathPhi7; _lineEditPath[7][5] = ui->lineEditPathTheta7; _lineEditPath[7][6] = ui->lineEditPathPsi7; + + for(int i=0; i<8; i++) { + for(int j=0; j<7; j++) { + _lineEditPath[i][j]->setValidator(new QDoubleValidator(this)); + } + } + + _interpolation.pathCount = 0; + _interpolation.node.clear(); + _interpolation.path.clear(); + + for(int i=0; i<8; i++) { + double a[7] = {0.0, }; + bool ok = false; + a[0] = _lineEditPath[i][0]->text().toDouble(&ok); + a[1] = _lineEditPath[i][1]->text().toDouble(&ok) / 1000.0; + a[2] = _lineEditPath[i][2]->text().toDouble(&ok) / 1000.0; + a[3] = _lineEditPath[i][3]->text().toDouble(&ok) / 1000.0; + a[4] = _lineEditPath[i][4]->text().toDouble(&ok) * _DEG2RAD; + a[5] = _lineEditPath[i][5]->text().toDouble(&ok) * _DEG2RAD; + a[6] = _lineEditPath[i][6]->text().toDouble(&ok) * _DEG2RAD; + _interpolation.init.push_back (std::valarray(&a[0], 7)); + } +} + +void MainWindow::printForwardKinematics() { + + dVector value = CTransformMatrix(_kinematics->Forward()).GetPositionOrientation(); + QString text; + + for(int i=0;i<6;i++) { + if(i < 3) { + text.sprintf("%4.0f",value[i] * 1000.0); + } else { + text.sprintf("%4.0f",value[i] * _RAD2DEG); } + + _labelForward[i]->setText(text); + } +} + +void MainWindow::printInverseKinematics() { + + dVector value = _kinematics->GetJointAngle(); + QString text; + + for(int i=0;i<5;i++) { + text.sprintf("%4.0f", value[i] * _RAD2DEG); + _labelInverse[i]->setText(text); } } diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.h b/PhantomX-Robot-Arm-Kit/mainwindow.h index 6dc9439..e56ceb3 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.h +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -1,15 +1,19 @@ #ifndef MAINWINDOW_H #define MAINWINDOW_H +#include +#include + #include #include #include #include #include -#include "serialport/serialport.h" #include "kinematics/posOriInverse.h" +#include "interpolation.h" #include "dynamixel/dynamixel.h" +#include "serialport/serialport.h" namespace Ui { class MainWindow; @@ -19,9 +23,10 @@ class MainWindow : public QMainWindow { Q_OBJECT - enum KinematicsIndex{ + enum TabIndex{ FORWARD = 0, - INVERSE = 1 + INVERSE = 1, + PATH = 2 }; typedef struct { @@ -30,6 +35,13 @@ class MainWindow : public QMainWindow dVector current; }Joint; + typedef struct { + std::vector> node; + std::vector> path; + std::vector> init; + uint32_t pathCount; + }Interpolation; + public: explicit MainWindow(QWidget *parent = 0); ~MainWindow(); @@ -44,6 +56,8 @@ class MainWindow : public QMainWindow QLineEdit **_lineEditInverse; QLabel ** _labelInverse; + QLineEdit ***_lineEditPath; + Serialport *_serialPort; QQueue *_messageQueue; QTimer *_taskTimer; @@ -54,6 +68,8 @@ class MainWindow : public QMainWindow Joint _q; dVector _target; + Interpolation _interpolation; + Dynamixel *_dynamixel; void printForwardKinematics(); @@ -61,6 +77,8 @@ class MainWindow : public QMainWindow dVector getForwardSliderValue(); dVector getInverseSliderValue(); + void setUiObject(); + public slots: void forwardValueChanged(int index, int val); void inverseValueChanged(int index, int val); @@ -69,6 +87,7 @@ private slots: void on_buttonReset_clicked(); void doUserTask(); + void on_buttonPathApply_clicked(); }; #endif // MAINWINDOW_H diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.ui b/PhantomX-Robot-Arm-Kit/mainwindow.ui index feba48a..8c8ba3e 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.ui +++ b/PhantomX-Robot-Arm-Kit/mainwindow.ui @@ -6,8 +6,8 @@ 0 0 - 791 - 520 + 776 + 408 @@ -52,6 +52,29 @@ QLayout::SetMinimumSize + + + + + 0 + 0 + + + + Reset + + + + + + + Axis: X(Red), Y(Green), Z(Blue) + + + Qt::AlignCenter + + + @@ -144,7 +167,7 @@ - Psi + ψ Qt::AlignCenter @@ -180,7 +203,7 @@ - X + x Qt::AlignCenter @@ -252,7 +275,7 @@ - Y + y Qt::AlignCenter @@ -429,7 +452,7 @@ - Phi + Φ Qt::AlignCenter @@ -571,7 +594,7 @@ - Theta + θ Qt::AlignCenter @@ -613,7 +636,7 @@ - Z + z Qt::AlignCenter @@ -797,7 +820,7 @@ - phi + Φ Qt::AlignCenter @@ -846,7 +869,10 @@ - theta + θ + + + Qt::AlignCenter @@ -1078,7 +1104,7 @@ - psi + ψ Qt::AlignCenter @@ -1256,29 +1282,667 @@ - - - - - - - 0 - 0 - - - - Reset - - - - - - - Axis: X(Red), Y(Green), Z(Blue) - - - Qt::AlignCenter - + + + Interpolation + + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 1.8 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + x(mm) + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + θ(°) + + + Qt::AlignCenter + + + + + + + 150 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + time(s) + + + Qt::AlignCenter + + + + + + + y(mm) + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Φ(°) + + + Qt::AlignCenter + + + + + + + z(mm) + + + Qt::AlignCenter + + + + + + + ψ(°) + + + Qt::AlignCenter + + + + + + + 150 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 150 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 150 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 150 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -150 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 1.2 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 1.5 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0.6 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 200 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0.3 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + apply + + + + + + + 0.9 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 2.1 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 100 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + -180 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + +