From ca2be6a18c49f7613d561f5366b01e0214aa5381 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Sun, 6 May 2018 23:09:37 +0900 Subject: [PATCH 1/8] =?UTF-8?q?Linear=20interpolation=20UI=20=EC=84=B8?= =?UTF-8?q?=ED=8C=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PhantomX-Robot-Arm-Kit/mainwindow.cpp | 16 +- PhantomX-Robot-Arm-Kit/mainwindow.h | 6 +- PhantomX-Robot-Arm-Kit/mainwindow.ui | 732 ++++++++++++++++++++++++-- 3 files changed, 712 insertions(+), 42 deletions(-) diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index a51a8d3..c0ae317 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -175,7 +175,7 @@ void MainWindow::printInverseKinematics() { QString text; for(int i=0;i<5;i++) { - text.sprintf("%4.0f",value[i] * _RAD2DEG); + text.sprintf("%4.0f", value[i] * _RAD2DEG); _labelInverse[i]->setText(text); } } @@ -242,10 +242,16 @@ void MainWindow::on_buttonReset_clicked() { } } +void MainWindow::on_pushButtonPathApply_clicked() +{ + +} + void MainWindow::doUserTask() { 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) { @@ -255,19 +261,18 @@ void MainWindow::doUserTask() { } } - _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; + _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(); + } else if(ui->tabWidget->currentIndex() == PATH) { + } if(_serialPort->isOpen()) { @@ -277,7 +282,6 @@ void MainWindow::doUserTask() { _kinematics->Forward(); ui->widget->updateGL(); - if(!_messageQueue->isEmpty()) { QByteArray buffer = _messageQueue->dequeue(); int16_t id = buffer[2]; diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.h b/PhantomX-Robot-Arm-Kit/mainwindow.h index 6dc9439..9b97a24 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.h +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -19,9 +19,10 @@ class MainWindow : public QMainWindow { Q_OBJECT - enum KinematicsIndex{ + enum TabIndex{ FORWARD = 0, - INVERSE = 1 + INVERSE = 1, + PATH = 2 }; typedef struct { @@ -69,6 +70,7 @@ private slots: void on_buttonReset_clicked(); void doUserTask(); + void on_pushButtonPathApply_clicked(); }; #endif // MAINWINDOW_H diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.ui b/PhantomX-Robot-Arm-Kit/mainwindow.ui index feba48a..1aa854f 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 - + + + Linear interpolation + + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 6 + + + 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 + + + + + + + 4 + + + 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 + + + + + + + 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 + + + + + + + 2 + + + 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 + + + + + + + 1 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + apply + + + + + + + 3 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 7 + + + 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 + + + + + + + From 74d73166a7601bbd6ae949c39cad169392376ca5 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Mon, 7 May 2018 00:46:15 +0900 Subject: [PATCH 2/8] =?UTF-8?q?node,=20path=20=EB=B3=80=EC=88=98=20?= =?UTF-8?q?=EC=84=A4=EC=A0=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PhantomX-Robot-Arm-Kit/glwidget.cpp | 36 ++- PhantomX-Robot-Arm-Kit/glwidget.h | 15 +- PhantomX-Robot-Arm-Kit/mainwindow.cpp | 329 +++++++++++++++----------- PhantomX-Robot-Arm-Kit/mainwindow.h | 7 + 4 files changed, 234 insertions(+), 153 deletions(-) diff --git a/PhantomX-Robot-Arm-Kit/glwidget.cpp b/PhantomX-Robot-Arm-Kit/glwidget.cpp index 728f1b9..14f84f4 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(.5, 0., .5); + renderPath (_node); + } + if(_path) { + glColor3d(1., 0., 1.); + 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(QVector> *path) { + _path = path; +} + +void GLWidget::setNode(QVector> *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(QVector> *path) +{ + std::vector line; + for (unsigned int i=0; isize(); ++i) { + QVector &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..7846d0f 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(QVector> *path); + void setNode(QVector> *node); void setKinematics(CKinematics *kinematics); void setJointAngle(dVector &q); @@ -58,17 +60,20 @@ class GLWidget : public QGLWidget // kinematics CKinematics *_kinematics; + QVector> *_node; + QVector> *_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(QVector> *path); + void renderTarget(); void renderJoint(); }; diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index c0ae317..f662c5b 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,67 @@ 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(); + } 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(); + } else if(ui->tabWidget->currentIndex() == PATH) { + } -} -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; + } } } @@ -242,71 +200,154 @@ void MainWindow::on_buttonReset_clicked() { } } -void MainWindow::on_pushButtonPathApply_clicked() -{ - +void MainWindow::on_pushButtonPathApply_clicked() { + _pathCount = 0; } -void MainWindow::doUserTask() { - 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]); +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<6; i++) { - if(i < 3) { - _sliderInverse[i]->setValue(value[i] * 1000.0); - } else { - _sliderInverse[i]->setValue(value[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; - printForwardKinematics(); - } 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); + _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++) { - _sliderForward[i]->setValue(_q.target[i] * _RAD2DEG); - } + for(int i=0; i<5; i++) { - printInverseKinematics(); - } else if(ui->tabWidget->currentIndex() == PATH) { + // 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); }); } - if(_serialPort->isOpen()) { - _serialPort->write(_dynamixel->generateJointAnglePacket(_q.target)); + // 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; + + _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); }); } - ui->widget->setJointAngle(_q.target); - _kinematics->Forward(); - ui->widget->updateGL(); - if(!_messageQueue->isEmpty()) { - QByteArray buffer = _messageQueue->dequeue(); - int16_t id = buffer[2]; - uint16_t pos = (((uint16_t)buffer[6] << 8) & 0xFF00) | (buffer[5] & 0xFF); + // set path + _pathCount = 0; - switch(id) { - case 1: - _q.current[0] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; - break; + _lineEditPath = new QLineEdit**[8]; + for(int i=0; i<8; i++) { + _lineEditPath[i] = new QLineEdit*[7]; + } - case 3: - _q.current[1] = (_dynamixel->convertDynamixelToAngle(pos) - Dynamixel::offset::LINK + Dynamixel::offset::ANGLE) * _DEG2RAD; - break; + _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)); + } + } +} - case 4: - _q.current[2] = (_dynamixel->convertDynamixelToAngle(pos) + Dynamixel::offset::LINK - Dynamixel::offset::ANGLE) * _DEG2RAD; - break; +void MainWindow::printForwardKinematics() { - case 6: - _q.current[3] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; - break; + dVector value = CTransformMatrix(_kinematics->Forward()).GetPositionOrientation(); + QString text; - case 7: - _q.current[4] = _dynamixel->convertDynamixelToAngle(pos) * _DEG2RAD; - break; + 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 9b97a24..d1083ac 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.h +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -45,6 +45,8 @@ class MainWindow : public QMainWindow QLineEdit **_lineEditInverse; QLabel ** _labelInverse; + QLineEdit ***_lineEditPath; + Serialport *_serialPort; QQueue *_messageQueue; QTimer *_taskTimer; @@ -55,6 +57,9 @@ class MainWindow : public QMainWindow Joint _q; dVector _target; + QVector> _node; + uint32_t _pathCount; + Dynamixel *_dynamixel; void printForwardKinematics(); @@ -62,6 +67,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); From 75d8708728e80fee3e778676c89766fecccb7a44 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Mon, 7 May 2018 01:41:53 +0900 Subject: [PATCH 3/8] =?UTF-8?q?apply=20=EB=B2=84=ED=8A=BC=20=ED=81=B4?= =?UTF-8?q?=EB=A6=AD=EC=8B=9C=20=EA=B2=BD=EB=A1=9C=20=EC=83=9D=EC=84=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PhantomX-Robot-Arm-Kit/glwidget.cpp | 4 ++-- PhantomX-Robot-Arm-Kit/mainwindow.cpp | 18 ++++++++++++++++++ PhantomX-Robot-Arm-Kit/mainwindow.h | 3 ++- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/PhantomX-Robot-Arm-Kit/glwidget.cpp b/PhantomX-Robot-Arm-Kit/glwidget.cpp index 14f84f4..ae747d1 100644 --- a/PhantomX-Robot-Arm-Kit/glwidget.cpp +++ b/PhantomX-Robot-Arm-Kit/glwidget.cpp @@ -55,11 +55,11 @@ void GLWidget::paintGL() { oglPlane(5.0, 0.5); if(_node) { - glColor3d(.5, 0., .5); + glColor3d(0.5, 0.0, 0.5); renderPath (_node); } if(_path) { - glColor3d(1., 0., 1.); + glColor3d(1.0, 0.0, 1.0); renderPath (_path); } diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index f662c5b..2cd74ac 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -201,7 +201,24 @@ void MainWindow::on_buttonReset_clicked() { } void MainWindow::on_pushButtonPathApply_clicked() { + bool ok = false; + _pathCount = 0; + _node->clear(); + + for(int i=0; i<8; i++) { + QVector node; + node.push_back(_lineEditPath[i][0]->text().toDouble(&ok)); + node.push_back(_lineEditPath[i][1]->text().toDouble(&ok) / 1000.0); + node.push_back(_lineEditPath[i][2]->text().toDouble(&ok) / 1000.0); + node.push_back(_lineEditPath[i][3]->text().toDouble(&ok) / 1000.0); + node.push_back(_lineEditPath[i][4]->text().toDouble(&ok) * _DEG2RAD); + node.push_back(_lineEditPath[i][5]->text().toDouble(&ok) * _DEG2RAD); + node.push_back(_lineEditPath[i][6]->text().toDouble(&ok) * _DEG2RAD); + _node->push_back(node); + } + + ui->widget->setNode(_node); } @@ -318,6 +335,7 @@ void MainWindow::setUiObject() { _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; + _node = new QVector>(8); for(int i=0; i<8; i++) { for(int j=0; j<7; j++) { _lineEditPath[i][j]->setValidator(new QDoubleValidator(this)); diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.h b/PhantomX-Robot-Arm-Kit/mainwindow.h index d1083ac..f06128e 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.h +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -6,6 +6,7 @@ #include #include #include +#include #include "serialport/serialport.h" #include "kinematics/posOriInverse.h" @@ -57,7 +58,7 @@ class MainWindow : public QMainWindow Joint _q; dVector _target; - QVector> _node; + QVector>* _node; uint32_t _pathCount; Dynamixel *_dynamixel; From f442fc5e914d2b945e4b128fcfcaf9c42b976473 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Mon, 7 May 2018 13:15:13 +0900 Subject: [PATCH 4/8] =?UTF-8?q?linearInterpolation=20=EB=94=B0=EB=9D=BC=20?= =?UTF-8?q?=EA=B2=BD=EB=A1=9C=EB=A5=BC=20=EC=83=9D=EC=84=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../PhantomX-Robot-Arm-Kit.pro | 6 ++- PhantomX-Robot-Arm-Kit/glwidget.cpp | 8 +-- PhantomX-Robot-Arm-Kit/glwidget.h | 10 ++-- PhantomX-Robot-Arm-Kit/interpolation.cpp | 31 +++++++++++ PhantomX-Robot-Arm-Kit/interpolation.h | 11 ++++ PhantomX-Robot-Arm-Kit/mainwindow.cpp | 52 ++++++++++++++----- PhantomX-Robot-Arm-Kit/mainwindow.h | 10 ++-- PhantomX-Robot-Arm-Kit/mainwindow.ui | 16 +++--- 8 files changed, 108 insertions(+), 36 deletions(-) create mode 100644 PhantomX-Robot-Arm-Kit/interpolation.cpp create mode 100644 PhantomX-Robot-Arm-Kit/interpolation.h 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 ae747d1..c5e3834 100644 --- a/PhantomX-Robot-Arm-Kit/glwidget.cpp +++ b/PhantomX-Robot-Arm-Kit/glwidget.cpp @@ -135,11 +135,11 @@ void GLWidget::setKinematics(CKinematics *kinematics) { _kinematics = kinematics; } -void GLWidget::setPath(QVector> *path) { +void GLWidget::setPath(std::vector> *path) { _path = path; } -void GLWidget::setNode(QVector> *node) { +void GLWidget::setNode(std::vector> *node) { _node = node; } @@ -167,11 +167,11 @@ void GLWidget::transformAxis(dMatrix A) { glMultMatrixd(m); } -void GLWidget::renderPath(QVector> *path) +void GLWidget::renderPath(std::vector> *path) { std::vector line; for (unsigned int i=0; isize(); ++i) { - QVector &p = (*path)[i]; + std::valarray &p = (*path)[i]; line.push_back (CPoint3d(p[1], p[2], p[3])); } oglLineStrip (line); diff --git a/PhantomX-Robot-Arm-Kit/glwidget.h b/PhantomX-Robot-Arm-Kit/glwidget.h index 7846d0f..b4eb886 100644 --- a/PhantomX-Robot-Arm-Kit/glwidget.h +++ b/PhantomX-Robot-Arm-Kit/glwidget.h @@ -27,8 +27,8 @@ class GLWidget : public QGLWidget public: explicit GLWidget(QWidget *parent = 0); - void setPath(QVector> *path); - void setNode(QVector> *node); + void setPath(std::vector> *path); + void setNode(std::vector> *node); void setKinematics(CKinematics *kinematics); void setJointAngle(dVector &q); @@ -60,8 +60,8 @@ class GLWidget : public QGLWidget // kinematics CKinematics *_kinematics; - QVector> *_node; - QVector> *_path; + std::vector> *_node; + std::vector> *_path; void setViewport(); @@ -72,7 +72,7 @@ class GLWidget : public QGLWidget void drawRevoluteJoint(JointInfo *joint); void drawPrismaticJoint(JointInfo *joint); - void renderPath(QVector> *path); + 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/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index 2cd74ac..88926b1 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -99,7 +99,25 @@ void MainWindow::doUserTask() { printInverseKinematics(); } else if(ui->tabWidget->currentIndex() == PATH) { + if(_path.size()) { + if(_pathCount >= _path.size()) { + _pathCount = 0; + } + + for(int i=0; i<6; i++) { + _target[i] = _path[_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); + } + + printInverseKinematics(); + _pathCount++; + } } if(_serialPort->isOpen()) { @@ -204,21 +222,25 @@ void MainWindow::on_pushButtonPathApply_clicked() { bool ok = false; _pathCount = 0; - _node->clear(); + _node.clear(); + _path.clear(); for(int i=0; i<8; i++) { - QVector node; - node.push_back(_lineEditPath[i][0]->text().toDouble(&ok)); - node.push_back(_lineEditPath[i][1]->text().toDouble(&ok) / 1000.0); - node.push_back(_lineEditPath[i][2]->text().toDouble(&ok) / 1000.0); - node.push_back(_lineEditPath[i][3]->text().toDouble(&ok) / 1000.0); - node.push_back(_lineEditPath[i][4]->text().toDouble(&ok) * _DEG2RAD); - node.push_back(_lineEditPath[i][5]->text().toDouble(&ok) * _DEG2RAD); - node.push_back(_lineEditPath[i][6]->text().toDouble(&ok) * _DEG2RAD); - _node->push_back(node); + 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; + _node.push_back (std::valarray(&a[0], 7)); } - ui->widget->setNode(_node); + _path = linearInterpolation(_node, (double)TASK_TIME/1000.0); + + ui->widget->setNode(&_node); + ui->widget->setPath(&_path); } @@ -301,8 +323,6 @@ void MainWindow::setUiObject() { } // set path - _pathCount = 0; - _lineEditPath = new QLineEdit**[8]; for(int i=0; i<8; i++) { _lineEditPath[i] = new QLineEdit*[7]; @@ -335,12 +355,16 @@ void MainWindow::setUiObject() { _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; - _node = new QVector>(8); for(int i=0; i<8; i++) { for(int j=0; j<7; j++) { _lineEditPath[i][j]->setValidator(new QDoubleValidator(this)); } } + + _pathCount = 0; + _node.clear(); + _path.clear(); + } void MainWindow::printForwardKinematics() { diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.h b/PhantomX-Robot-Arm-Kit/mainwindow.h index f06128e..496b5d2 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.h +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -1,16 +1,19 @@ #ifndef MAINWINDOW_H #define MAINWINDOW_H +#include +#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; @@ -58,7 +61,8 @@ class MainWindow : public QMainWindow Joint _q; dVector _target; - QVector>* _node; + std::vector> _node; + std::vector> _path; uint32_t _pathCount; Dynamixel *_dynamixel; diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.ui b/PhantomX-Robot-Arm-Kit/mainwindow.ui index 1aa854f..62d17cf 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.ui +++ b/PhantomX-Robot-Arm-Kit/mainwindow.ui @@ -118,7 +118,7 @@ Qt::LeftToRight - 0 + 2 Qt::ElideRight @@ -1302,7 +1302,7 @@ - 6 + 1.8 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1602,7 +1602,7 @@ - 4 + 1.2 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1642,7 +1642,7 @@ - 5 + 1.5 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1775,7 +1775,7 @@ - 2 + 0.6 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1835,7 +1835,7 @@ - 1 + 0.3 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1862,7 +1862,7 @@ - 3 + 0.9 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1872,7 +1872,7 @@ - 7 + 2.1 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter From cc98152a2fafe70b83385fa7af3bbe24a589bbfd Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Sat, 12 May 2018 13:40:26 +0900 Subject: [PATCH 5/8] =?UTF-8?q?reset=20=EB=B2=84=ED=8A=BC=EC=9D=84=20?= =?UTF-8?q?=EB=88=84=EB=A6=B4=EB=95=8C=20=EA=B0=92=EC=9D=B4=20=EC=95=88?= =?UTF-8?q?=EB=A7=9E=EB=8A=94=20=EA=B2=83=EC=9D=84=20=EC=88=98=EC=A0=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PhantomX-Robot-Arm-Kit/mainwindow.cpp | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index 88926b1..1189ae5 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -181,16 +181,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 { @@ -201,21 +197,19 @@ 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)); } } + ui->widget->updateGL(); } void MainWindow::on_pushButtonPathApply_clicked() { From ae3e4a577cce8bdad077db0cec149c1b486e76c1 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Sat, 12 May 2018 13:41:16 +0900 Subject: [PATCH 6/8] =?UTF-8?q?reset=20=EB=B2=84=ED=8A=BC=EC=9D=84=20?= =?UTF-8?q?=EB=88=84=EB=A5=BC=EB=95=8C=20=EA=B2=BD=EB=A1=9C=EB=8F=84=20?= =?UTF-8?q?=EC=B4=88=EA=B8=B0=ED=99=94=20=ED=95=B4=EC=A4=80=EB=8B=A4.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PhantomX-Robot-Arm-Kit/mainwindow.cpp | 75 +++++++++++++++++++++------ PhantomX-Robot-Arm-Kit/mainwindow.h | 13 +++-- PhantomX-Robot-Arm-Kit/mainwindow.ui | 4 +- 3 files changed, 70 insertions(+), 22 deletions(-) diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index 1189ae5..51cad13 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -89,6 +89,10 @@ void MainWindow::doUserTask() { } 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); @@ -98,14 +102,18 @@ void MainWindow::doUserTask() { } printInverseKinematics(); + ui->widget->setNode(NULL); + ui->widget->setPath(NULL); + _interpolation.node.clear(); + _interpolation.path.clear(); } else if(ui->tabWidget->currentIndex() == PATH) { - if(_path.size()) { - if(_pathCount >= _path.size()) { - _pathCount = 0; + if(_interpolation.path.size()) { + if(_interpolation.pathCount >= _interpolation.path.size()) { + _interpolation.pathCount = 0; } for(int i=0; i<6; i++) { - _target[i] = _path[_pathCount][i+1]; + _target[i] = _interpolation.path[_interpolation.pathCount][i+1]; } _kinematics->SetDesired(_target[0], _target[1], _target[2], _target[3], _target[4], _target[5]); @@ -115,8 +123,17 @@ void MainWindow::doUserTask() { _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(); - _pathCount++; + _interpolation.pathCount++; } } @@ -210,14 +227,28 @@ void MainWindow::on_buttonReset_clicked() { } } 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::on_pushButtonPathApply_clicked() { +void MainWindow::on_buttonPathApply_clicked() { bool ok = false; - _pathCount = 0; - _node.clear(); - _path.clear(); + _interpolation.pathCount = 0; + _interpolation.node.clear(); + _interpolation.path.clear(); for(int i=0; i<8; i++) { double a[7] = {0.0, }; @@ -228,13 +259,13 @@ void MainWindow::on_pushButtonPathApply_clicked() { 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; - _node.push_back (std::valarray(&a[0], 7)); + _interpolation.node.push_back (std::valarray(&a[0], 7)); } - _path = linearInterpolation(_node, (double)TASK_TIME/1000.0); + _interpolation.path = linearInterpolation(_interpolation.node, (double)TASK_TIME/1000.0); - ui->widget->setNode(&_node); - ui->widget->setPath(&_path); + ui->widget->setNode(&_interpolation.node); + ui->widget->setPath(&_interpolation.path); } @@ -355,10 +386,22 @@ void MainWindow::setUiObject() { } } - _pathCount = 0; - _node.clear(); - _path.clear(); + _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() { diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.h b/PhantomX-Robot-Arm-Kit/mainwindow.h index 496b5d2..e56ceb3 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.h +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -35,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(); @@ -61,9 +68,7 @@ class MainWindow : public QMainWindow Joint _q; dVector _target; - std::vector> _node; - std::vector> _path; - uint32_t _pathCount; + Interpolation _interpolation; Dynamixel *_dynamixel; @@ -82,7 +87,7 @@ private slots: void on_buttonReset_clicked(); void doUserTask(); - void on_pushButtonPathApply_clicked(); + void on_buttonPathApply_clicked(); }; #endif // MAINWINDOW_H diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.ui b/PhantomX-Robot-Arm-Kit/mainwindow.ui index 62d17cf..36739ce 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.ui +++ b/PhantomX-Robot-Arm-Kit/mainwindow.ui @@ -118,7 +118,7 @@ Qt::LeftToRight - 2 + 0 Qt::ElideRight @@ -1853,7 +1853,7 @@ - + apply From 3b9c0b7335160e9e5b50a8f8b002d8ae909f6d81 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Sat, 12 May 2018 13:47:59 +0900 Subject: [PATCH 7/8] =?UTF-8?q?linearInterpolation=20=ED=83=AD=EC=97=90?= =?UTF-8?q?=EC=84=9C=20reset=EC=9D=84=20=EB=88=84=EB=A5=BC=20=EB=96=84=20D?= =?UTF-8?q?esired=20=EC=9E=AC=EC=84=A4=EC=A0=95=20=EC=B6=94=EA=B0=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PhantomX-Robot-Arm-Kit/mainwindow.cpp | 2 ++ PhantomX-Robot-Arm-Kit/mainwindow.ui | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp index 51cad13..373925b 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.cpp +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -226,6 +226,8 @@ void MainWindow::on_buttonReset_clicked() { _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; diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.ui b/PhantomX-Robot-Arm-Kit/mainwindow.ui index 36739ce..8c8ba3e 100644 --- a/PhantomX-Robot-Arm-Kit/mainwindow.ui +++ b/PhantomX-Robot-Arm-Kit/mainwindow.ui @@ -1282,9 +1282,9 @@ - + - Linear interpolation + Interpolation From 6f0176168cc9ef10c3f603535599051802418076 Mon Sep 17 00:00:00 2001 From: Jihun Kim Date: Sat, 12 May 2018 15:42:08 +0900 Subject: [PATCH 8/8] Change the version to v1.3.0 --- PhantomX-Robot-Arm-Kit/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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[]) {