Merge branch 'release/v1.3.0'

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

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<std::valarray<double>> *path) {
+    _path = path;
+}
+
+void GLWidget::setNode(std::vector<std::valarray<double>> *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<std::valarray<double>> *path)
+{
+    std::vector<CPoint3d> line;
+    for (unsigned int i=0; i<path->size(); ++i) {
+        std::valarray<double> &p = (*path)[i];
+        line.push_back (CPoint3d(p[1], p[2], p[3]));
+    }
+    oglLineStrip (line);
+}
+
+void GLWidget::renderTarget() {
     dVector desired = _kinematics->GetDesired ();
 
     glPushMatrix();

PhantomX-Robot-Arm-Kit/glwidget.h

@@ -27,6 +27,8 @@ class GLWidget : public QGLWidget
 public:
     explicit GLWidget(QWidget *parent = 0);
 
+    void setPath(std::vector<std::valarray<double>> *path);
+    void setNode(std::vector<std::valarray<double>> *node);
     void setKinematics(CKinematics *kinematics);
     void setJointAngle(dVector &q);
 
@@ -58,17 +60,20 @@ class GLWidget : public QGLWidget
 
     // kinematics
     CKinematics *_kinematics;
+    std::vector<std::valarray<double>> *_node;
+    std::vector<std::valarray<double>> *_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<std::valarray<double>> *path);
+    void renderTarget();
     void renderJoint();
 };
 

PhantomX-Robot-Arm-Kit/interpolation.cpp

@@ -0,0 +1,31 @@
+#include "interpolation.h"
+
+std::vector<std::valarray<double>> linearInterpolation (std::vector<std::valarray<double>> &x, double timeSlice) {
+    if (timeSlice < 0.001) timeSlice = 0.001;
+    if (x.size() < 2) return x;
+
+    unsigned int n = x.size () - 1;
+
+    std::vector<double> h(n);
+    for (unsigned int i=0; i<n; i++) {
+        h[i] = x[i+1][0] - x[i][0];
+        if (h[i] < timeSlice) h[i] = timeSlice;
+    }
+
+    std::vector<std::valarray<double>> s;
+    s.reserve ((int)(x[n][0] / timeSlice + 10));
+
+    double t = x[0][0];
+    for (unsigned int i=0; i<n; i++) {
+        std::valarray<double> m = (x[i+1] - x[i])/h[i];
+
+        for (; t<=x[i+1][0]; t+=timeSlice) {
+            std::valarray<double> p = m*(t - x[i][0]) + x[i];
+            p[0] = t;
+            s.push_back (p);
+        }
+    }
+    s.push_back (x[n]);
+
+    return s;
+}

PhantomX-Robot-Arm-Kit/interpolation.h

@@ -0,0 +1,11 @@
+#ifndef INTERPOLATION_H
+#define INTERPOLATION_H
+
+#include <vector>
+#include <valarray>
+
+#include <QDebug>
+
+std::vector<std::valarray<double>> linearInterpolation (std::vector<std::valarray<double>> &x, double timeSlice);
+
+#endif // INTERPOLATION_H

PhantomX-Robot-Arm-Kit/main.cpp

@@ -1,7 +1,7 @@
 #include "mainwindow.h"
 #include <QApplication>
 
-#define APP_VERSION "1.2.0"
+#define APP_VERSION "1.3.0"
 
 int main(int argc, char *argv[])
 {