diff --git a/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro b/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro new file mode 100644 index 0000000..e7c1c25 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/PhantomX-Robot-Arm-Kit.pro @@ -0,0 +1,58 @@ +#------------------------------------------------- +# +# Project created by QtCreator 2018-03-13T11:20:44 +# +#------------------------------------------------- + +QT += core gui opengl + +greaterThan(QT_MAJOR_VERSION, 4): QT += widgets + +TARGET = PhantomX-Robot-Arm-Kit +TEMPLATE = app + +# The following define makes your compiler emit warnings if you use +# any feature of Qt which has been marked as deprecated (the exact warnings +# depend on your compiler). Please consult the documentation of the +# deprecated API in order to know how to port your code away from it. +DEFINES += QT_DEPRECATED_WARNINGS + +# You can also make your code fail to compile if you use deprecated APIs. +# In order to do so, uncomment the following line. +# You can also select to disable deprecated APIs only up to a certain version of Qt. +#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 + + +SOURCES += \ + main.cpp \ + mainwindow.cpp \ + glwidget.cpp \ + kinematics/kinematics.cpp \ + kinematics/posOriInverse.cpp \ + kinematics/quaternion.cpp \ + matrix/transformation.cpp \ + opengl/oglObjects.cpp + +HEADERS += \ + mainwindow.h \ + glwidget.h \ + kinematics/kinematics.h \ + kinematics/posOriInverse.h \ + kinematics/quaternion.h \ + matrix/mat/matrix.h \ + matrix/mathDef.h \ + matrix/matrixAlgebra.h \ + matrix/transformation.h \ + opengl/oglDef.h \ + opengl/oglObjects.h + +FORMS += \ + mainwindow.ui + +win32 { + LIBS += -lOpengl32 \ + -lglu32 +} + +RESOURCES += \ + resource.qrc diff --git a/PhantomX-Robot-Arm-Kit/glwidget.cpp b/PhantomX-Robot-Arm-Kit/glwidget.cpp new file mode 100644 index 0000000..ef84800 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/glwidget.cpp @@ -0,0 +1,259 @@ +#include "glwidget.h" + +GLWidget::GLWidget(QWidget *parent) : + QGLWidget(parent) { + + _eyePos = CPoint3d(1.1, -0.03, 0.4); + _centerPos = CPoint3d(0, 0, 0.14); + _angleHor = -35.0; + _angleVer = 10.5; + _fovAngle = 45.0; +} + +void GLWidget::initializeGL() { + + glClearColor(240.0f,240.0f,240.0f, 1.0); + glClearDepth(1.0); + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LEQUAL); + glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); + + // 조명 설정 + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); + + float AmbientColor[] = { 0.0f, 0.1f, 0.2f, 0.0f }; + float DiffuseColor[] = { 0.5f, 0.5f, 0.5f, 0.0f }; + float SpecularColor[] = { 0.5f, 0.5f, 0.5f, 0.0f }; + float Position[] = { 100.0f, 100.0f, -400.0f, 1.0f }; + + glLightfv(GL_LIGHT1, GL_AMBIENT, AmbientColor); + glLightfv(GL_LIGHT1, GL_DIFFUSE, DiffuseColor); + glLightfv(GL_LIGHT1, GL_SPECULAR, SpecularColor); + glLightfv(GL_LIGHT1, GL_POSITION, Position); + + // 재질의 속성 설정 + glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE); + glEnable(GL_COLOR_MATERIAL); +} + +void GLWidget::paintGL() { + + // 그림 그리기 + glMatrixMode(GL_MODELVIEW); // Select The Modelview Matrix + glLoadIdentity(); // Reset The Modelview Matrix + + // 그림 지우기 + // Clear The Screen And The Depth Buffer + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + setViewport(); + oglPlane(5.0, 0.5); + + renderTarget(); + renderJoint(); + + glFinish(); +} + +void GLWidget::resizeGL(int width, int height) { + + if (width < 1) width = 1; + if (height < 1) height = 1; + + glViewport(0, 0, width, height); + + glMatrixMode(GL_PROJECTION); // Select The Projection Matrix + glLoadIdentity(); // Reset The Projection Matrix + + _glnWidth = width; + _glnHeight = height; + + gluPerspective(_fovAngle, (double)_glnWidth/_glnHeight, 0.1, 1000000.0); +} + +void GLWidget::mouseMoveEvent(QMouseEvent *event) { + + if(event->buttons() == Qt::LeftButton) { + _angleHor+=(event->localPos().x()-_mouseDownPoint.x())/3.6; + _angleVer+=(event->localPos().y()-_mouseDownPoint.y())/3.6; + } else if(event->buttons() == Qt::RightButton) { + _centerPos.y -= (event->localPos().x()-_mouseDownPoint.x())/100.0; + _centerPos.z += (event->localPos().y()-_mouseDownPoint.y())/100.0; + } + + _mouseDownPoint=event->localPos(); + + updateGL(); +} + +void GLWidget::mousePressEvent(QMouseEvent *event) { + + + if(event->buttons() != Qt::MiddleButton) { + _mouseDownPoint = event->localPos(); + } else { + _eyePos = CPoint3d(1.1, -0.03, 0.4); + _centerPos = CPoint3d(0, 0, 0.14); + _angleHor = -35.0; + _angleVer = 10.5; + _fovAngle = 45.0; + updateGL(); + } +} + +void GLWidget::wheelEvent(QWheelEvent *event) { + + _eyePos.x -= _centerPos.x; + _eyePos.y -= _centerPos.y; + _eyePos.z -= _centerPos.z; + + _eyePos.x *= (event->delta() < 0) ? 0.9 : 1.1; + _eyePos.y *= (event->delta() < 0) ? 0.9 : 1.1; + _eyePos.z *= (event->delta() < 0) ? 0.9 : 1.1; + + _eyePos.x += _centerPos.x; + _eyePos.y += _centerPos.y; + _eyePos.z += _centerPos.z; + + updateGL(); +} + +void GLWidget::setKinematics(CKinematics *kinematics) { + _kinematics = kinematics; +} + +void GLWidget::setJointAngle(dVector &q) +{ + _kinematics->SetJointAngle(q); +} + +void GLWidget::setViewport() { + // 보는 시각 설정 + gluLookAt( _eyePos.x, _eyePos.y, _eyePos.z, _centerPos.x, _centerPos.y, _centerPos.z, 0,0,1 ); + + // 좌표계 회전 + glRotated( _angleVer, 0,1,0 ); + glRotated( _angleHor, 0,0,1 ); +} + +void GLWidget::transformAxis(dMatrix A) { + double m[16]; + + m[0] = A(0,0); m[4] = A(0,1); m[8] = A(0,2); m[12] = A(0,3); + m[1] = A(1,0); m[5] = A(1,1); m[9] = A(1,2); m[13] = A(1,3); + m[2] = A(2,0); m[6] = A(2,1); m[10]= A(2,2); m[14] = A(2,3); + m[3] = 0.0; m[7] = 0.0; m[11]= 0.0; m[15] = 1.0; + + glMultMatrixd(m); +} + +void GLWidget::renderTarget () { + dVector desired = _kinematics->GetDesired (); + + glPushMatrix(); + + glTranslated(desired[0], desired[1], desired[2]); + glRotated (_RAD2DEG*desired[5], 0, 0, 1); + glRotated (_RAD2DEG*desired[4], 0, 1, 0); + glRotated (_RAD2DEG*desired[3], 1, 0, 0); + + glColor3d(1.0, 0., 0.); + oglBox (0.06, 0.06, 0.06); + + oglCoordinate (0.3); + + glPopMatrix(); +} + +void GLWidget::drawRevLink (double x, double y, double z, double radius) { + glPushMatrix(); + + double rz = _RAD2DEG*atan2(y, x); + double ry = 90. - _RAD2DEG*atan2(z, sqrt(x*x + y*y)); + double height = sqrt (x*x + y*y + z*z); + + glRotated( rz, 0, 0, 1); + glRotated( ry, 0, 1, 0); + glTranslated (0, 0, height/2.); + + oglCylinder (height, radius); + + glPopMatrix(); +} + +void GLWidget::drawFixedJoint(JointInfo *joint) { + glColor3d (0.7, 0.7, 0.7); + drawRevLink (joint->x, joint->y, joint->z, joint->radius); +} + +void GLWidget::drawRevoluteJoint(JointInfo *joint) { + glPushMatrix(); + + if (joint->axis == 0) glRotated( 90., 0,1,0 ); + else if (joint->axis == 1) glRotated(-90., 1,0,0 ); + + glColor3d(0.5, 0.5, 0.5); + oglCylinder (joint->radius*2.5, joint->radius*1.3); + + glPopMatrix(); + + glPushMatrix(); + + transformAxis(joint->TransformationMatrixQ()); + + glColor3d(0.7, 0.7, 0.7); + drawRevLink (joint->x, joint->y, joint->z, joint->radius); + + glPopMatrix(); +} + +void GLWidget::drawPrismaticJoint(JointInfo *joint) { + glPushMatrix(); + + if (joint->axis == 0) glRotated( 90., 0,1,0 ); + else if (joint->axis == 1) glRotated(-90., 1,0,0 ); + + glColor3d(0.5, 0.5, 0.5); + oglBox (joint->radius*2.5, joint->radius*2.5, joint->radius*2.5); + glTranslated (0, 0, joint->q/2.); + + glColor3d(0.8, 0.8, 0.8); + oglBox (joint->radius*1.5, joint->radius*1.5, joint->q); + + glPopMatrix(); + + glPushMatrix(); + + transformAxis(joint->TransformationMatrixQ()); + + glColor3d(0.7, 0.7, 0.7); + drawRevLink (joint->x, joint->y, joint->z, joint->radius); + + glPopMatrix(); +} + +void GLWidget::renderJoint() { + vector &jointList = _kinematics->GetJointList (); + + for (vector::iterator it = jointList.begin (); it != jointList.end (); ++it) { + JointInfo *joint = *it; + + glColor3d(0.7, 0.7, 0.7); + + if (joint->type == FIXED_JOINT) { + drawFixedJoint (joint); + } + else if (joint->type == REVOLUTE_JOINT) { + drawRevoluteJoint (joint); + } + else if (joint->type == PRISMATIC_JOINT) { + drawPrismaticJoint (joint); + } + + transformAxis(joint->TransformationMatrix()); + } + + oglCoordinate (0.3); +} diff --git a/PhantomX-Robot-Arm-Kit/glwidget.h b/PhantomX-Robot-Arm-Kit/glwidget.h new file mode 100644 index 0000000..71269fd --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/glwidget.h @@ -0,0 +1,75 @@ +#ifndef GLWIDGET_H +#define GLWIDGET_H + +#include +#include +#include +#include +#include + +#if defined(_WIN32) || defined(_WIN64) +#include +#elif defined(__APPLE__) +#include +#endif + +#include "opengl/oglDef.h" +#include "kinematics/kinematics.h" +#include "kinematics/posOriInverse.h" + +#include "matrix/mathDef.h" +#include "matrix/transformation.h" +#include "opengl/oglObjects.h" + +class GLWidget : public QGLWidget +{ + Q_OBJECT +public: + explicit GLWidget(QWidget *parent = 0); + + void setKinematics(CKinematics *kinematics); + void setJointAngle(dVector &q); + + void mouseMoveEvent(QMouseEvent *event); + void mousePressEvent(QMouseEvent *event); + void wheelEvent(QWheelEvent *event); + + void initializeGL(); + void paintGL(); + void resizeGL(int width, int height); + +private: + + // 아래 변수들로부터 로봇을 바라보는 눈의 위치와 방향을 설정한다. + // 눈의 위치(_eyePos)로부터 물체의 중심(_centerPos)을 바라보는데 수평(_angleHor)과 + // 수직(_angleVer)로 물체를 회전한다. + CPoint3d _eyePos; + CPoint3d _centerPos; + double _angleHor; + double _angleVer; + + // Perspective projection 설정 + double _fovAngle; + int _glnWidth; + int _glnHeight; + + // Mouse 설정 + QPointF _mouseDownPoint; + + // kinematics + CKinematics *_kinematics; + + void setViewport(); + + void transformAxis(dMatrix A); + void drawRevLink (double x, double y, double z, double radius); + + void drawFixedJoint (JointInfo *joint); + void drawRevoluteJoint (JointInfo *joint); + void drawPrismaticJoint (JointInfo *joint); + + void renderTarget (); + void renderJoint(); +}; + +#endif // GLWIDGET_H diff --git a/PhantomX-Robot-Arm-Kit/image/phantomX-robot-arm-kit.png b/PhantomX-Robot-Arm-Kit/image/phantomX-robot-arm-kit.png new file mode 100644 index 0000000..3809305 Binary files /dev/null and b/PhantomX-Robot-Arm-Kit/image/phantomX-robot-arm-kit.png differ diff --git a/PhantomX-Robot-Arm-Kit/kinematics/kinematics.cpp b/PhantomX-Robot-Arm-Kit/kinematics/kinematics.cpp new file mode 100644 index 0000000..4628347 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/kinematics/kinematics.cpp @@ -0,0 +1,242 @@ +////////////////////////////////////////////////////// +// Kinematics files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#include "kinematics.h" + +CTransformMatrix JointInfo::TransformationMatrixQ() +{ + if (type == PRISMATIC_JOINT){ + if (axis == 0) return CTransformMatrix(q, 0, 0, 0, 0, 0); + else if (axis == 1) return CTransformMatrix(0, q, 0, 0, 0, 0); + else if (axis == 2) return CTransformMatrix(0, 0, q, 0, 0, 0); + } + else if (type == REVOLUTE_JOINT) { + if (axis == 0) return CTransformMatrix(0, 0, 0, q, 0, 0); + else if (axis == 1) return CTransformMatrix(0, 0, 0, 0, q, 0); + else if (axis == 2) return CTransformMatrix(0, 0, 0, 0, 0, q); + } + + return CTransformMatrix(0, 0, 0, 0, 0, 0); +} + +CTransformMatrix JointInfo::TransformationMatrix() +{ + return TransformationMatrixQ() * CTransformMatrix(x, y, z, phi, theta, psi); +} + + +CKinematics::CKinematics(int taskSize) : + _desired(taskSize), _current(taskSize) +{ + _desired.zero(); + _current.zero(); +} + +CKinematics::~CKinematics(void) +{ + for (vector::iterator it = _jointList.begin(); it != _jointList.end(); ++it) { + delete *it; + } +} + +void CKinematics::AttachJoint (eJointType type, int axis, double x, double y, double z, double phi, double theta, double psi, + double radius, double limit_lo, double limit_hi, double median, double weight) +{ + JointInfo *joint = new JointInfo; + + joint->type = type; + joint->axis = axis; + + joint->x = x; + joint->y = y; + joint->z = z; + joint->phi = phi; + joint->theta = theta; + joint->psi = psi; + + joint->radius = radius; + + joint->limit_lo = limit_lo; + joint->limit_hi = limit_hi; + joint->median = median; + joint->weight = weight; + + _jointList.push_back(joint); +} + +double CKinematics::WeightFunction (double lo, double hi, double q) +{ + if (lo < hi) { + if (q < lo) q = lo; + if (q > hi) q = hi; + + // w = 2(q - lo)/(hi - lo) -1 // 회전각의 범위 [lo, hi]을 [-1, 1]로 스케일 변환한 q의 위치를 계산 + // w = w^32 // U자 모양의 곡선으로 바꾼다. + double w = 2*(q - lo)/(hi - lo) - 1; + w *= w; w *= w; w *= w; w *= w; w *= w; + return w; + } + else { + return 1.; + } +} + +dMatrix CKinematics::WeightMatrix (unsigned int dof) +{ + dMatrix W(dof, dof); + W.unit(); + +/* + if (_weightCur.size() != dof) { + _weightCur.resize (dof); + + for (unsigned int i=0; iw; + } + } + + _weightPrv = _weightCur; + + for (unsigned int i=0; i<_jointList.size () && iw*(1. - WeightFunction(joint->lo, joint->hi, _kin->GetJointAngle(i))); + // 1 step 이전 weight(_weightPrv)값과 현재 계산된 weight(_weightCur)값을 비교하여 + // weight가 증가할 때는 경계 근처로 가는 경우이므로 weight 값을 그대로 두고 + // weight가 감소할 때는 경계에서 빠져나오는 경우이므로 weight 값을 최고값으로 한다. + W(i,i) = (_weightPrv[i] < _weightCur[i]) ? joint->w : _weightCur[i]; + } +*/ + for (unsigned int i=0; i<_jointList.size() && iweight; + } + return W; +} + +dVector CKinematics::NullSpaceVector() +{ + dVector h(_jointList.size ()); + + for (unsigned int i=0; i<_jointList.size(); ++i) { + JointInfo *joint = _jointList[i]; + + // Joint의 회전각(q)이 기저값(basis: 사용자가 설정한 로봇의 기본 자세) 근처로 수렴하도록 한다 + h[i] = joint->median - joint->q; + } + + return h; +} + +void CKinematics::ClampMag(dVector &w, double maxMag) +{ + double mag = Norm2(w); + + if (maxMag < mag){ + w *= maxMag/mag; + } +} + +dVector CKinematics::SolveJTR(double minAlpha, double clampTask) +{ + Forward(); + dMatrix J = Jacobian(); + dVector de = Error(); + + ClampMag(de, clampTask); + + // Compute alpha constant as quotient of 2 inner products as suggested by + // Buss & Kim, to minimze de after caller updates theta + dVector JJtde = (J * ~J) * de; + double alpha = Dot(de, JJtde)/Dot(JJtde, JJtde); + + if (alpha < minAlpha) { + alpha = minAlpha; + } + + return alpha * ~J * de; +} + +dVector CKinematics::SolveDLS(double dlsLambda, double clampTask, double clampNull) +{ + Forward(); + dMatrix J = Jacobian(); + dVector de = Error(); + + ClampMag(de, clampTask); + + dMatrix invJ; + + // Moore-Penrose pseudo-inverse: + // case m < n: invA = ~A * !(A * ~A) + if (J.rowno() <= J.colno()) { + dMatrix I(J.rowno(), J.rowno()); + I.unit(); + // Damped least-square method + invJ = ~J * !( J*~J + dlsLambda*dlsLambda*I ); + } + // case m > n: invA = !(~A * A) * ~A + else { + dMatrix I(J.colno(), J.colno()); + I.unit(); + // Damped least-square method + invJ = !( ~J*J + dlsLambda*dlsLambda*I ) * ~J; + } + + dVector dq = invJ * de; + + if (0. < clampNull) { + dVector null = NullSpaceVector(); + + ClampMag(null, clampNull); + + dMatrix I(J.colno(), J.colno()); + I.unit(); + + dq += (I - invJ*J)*null; + } + + return dq; +} + +dVector CKinematics::GetJointAngle() +{ + dVector ang(_jointList.size()); + + for (unsigned int i=0; i<_jointList.size(); ++i) { + JointInfo *joint = _jointList[i]; + ang[i] = joint->q; + } + return ang; +} + +void CKinematics::SetJointAngle(dVector &q) +{ + for (unsigned int i = 0; i<_jointList.size() && iq = q[i]; + if (joint->limit_lo < joint->limit_hi) { + joint->q = Limit(joint->q, joint->limit_lo, joint->limit_hi); + } + } +} + +void CKinematics::Apply(dVector &dq) +{ + for (unsigned int i=0; i<_jointList.size() && iq += dq[i]; + if (joint->limit_lo < joint->limit_hi) { + joint->q = Limit (joint->q, joint->limit_lo, joint->limit_hi); + } + } +} + diff --git a/PhantomX-Robot-Arm-Kit/kinematics/kinematics.h b/PhantomX-Robot-Arm-Kit/kinematics/kinematics.h new file mode 100644 index 0000000..3b31826 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/kinematics/kinematics.h @@ -0,0 +1,87 @@ +////////////////////////////////////////////////////// +// Kinematics files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#pragma once + +#include + +#include "matrix/transformation.h" +#include "matrix/mathDef.h" + +using namespace std; + + +enum eJointType { + FIXED_JOINT, // 고정된 조인트: 좌표를 이동하기위해 사용한다. + REVOLUTE_JOINT, // 회전 운동 조인트 + PRISMATIC_JOINT, // 직선 운동 조인트 +}; + +struct JointInfo { + // 조인트의 초기 설정치들 + eJointType type; // 종류 + int axis; // 회전(직선) 운동 중심 축 지정 (0 - x축, 1 - y축, 2 - z축) + + double x, y, z; // 이전 조인트 위치로부터 현재 조인트의 상대 위치 + double phi, theta, psi; // 이전 조인트 자세로부터 현재 조인트의 상대 자세 + double radius; // 링크의 굵기, OpenGL에서 그리기 위해서 사용함 + + // 프로그램 실행 중 사용되는 변수들 + double q; // 조인트의 현재 회전(직선) 위치 + double limit_lo, limit_hi; // 조인트의 구동 범위 (limit_lo ~ limit_hi) + double median; // 조인트의 중간 값 (Null motion을 할당하기 위해 사용됨) + double weight; // 조인트의 가중치 + dVector a; // 회전 축의 각속도 + dVector p; // 전역 좌표계에서 회전 축의 위치 + + JointInfo() : q(0.), limit_lo(-M_PI), limit_hi(M_PI), median(0.), weight(0.), a(3), p(3) { } + + CTransformMatrix TransformationMatrixQ(); + CTransformMatrix TransformationMatrix(); +}; + + +class CKinematics +{ +public: + CKinematics(int taskSize); + ~CKinematics(void); + + virtual dMatrix Forward() = 0; + virtual dVector Error() = 0; + virtual dMatrix Jacobian() = 0; + + dVector GetDesired() { return _desired; } // 목적지의 위치를 반환한다. + dVector GetCurrent() { return _current; } // 현재 위치를 반환한다. + + // 조인트 목록 끝에 하나의 조인트를 더한다. + void AttachJoint (eJointType type, int axis, double x, double y, double z, double phi, double theta, double psi, + double radius, double limit_lo = -M_PI, double limit_hi = M_PI, double median = 0., double weight = 1.); + + dVector SolveJTR (double minAlpha, double clampTask); // Jacobian Transpose 방법으로 Inverse kinematics를 푼다. + dVector SolveDLS (double dlsLambda, double clampTask, double clampNull); // Damped least square 방법으로 Inverse kinematics를 푼다. + + dVector GetJointAngle(); // 현재 조인트 각도(q) 집합을 반환한다. + void SetJointAngle(dVector &q); + void Apply(dVector &dq); // 각 조인트의 회전량(dq)를 현재 조인트 각도(q)에 더한다. + + vector &GetJointList() { return _jointList; } // 조인트 정보를 담고있는 목록을 얻어온다. (그리기 위해 사용) + +protected: + vector _jointList; + + dVector _desired; + dVector _current; + + void ClampMag(dVector &w, double maxMag); + + double WeightFunction (double lo, double hi, double q); + dMatrix WeightMatrix (unsigned int dof); + dVector NullSpaceVector (); +}; diff --git a/PhantomX-Robot-Arm-Kit/kinematics/posOriInverse.cpp b/PhantomX-Robot-Arm-Kit/kinematics/posOriInverse.cpp new file mode 100644 index 0000000..b5c6635 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/kinematics/posOriInverse.cpp @@ -0,0 +1,136 @@ +////////////////////////////////////////////////////// +// Kinematics files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#include "posOriInverse.h" +#include "matrix/transformation.h" +#include "matrix/mathDef.h" +#include "quaternion.h" + +CPosOriInverse::CPosOriInverse (int inverseAxis) +: CKinematics(6), _inverseAxis(inverseAxis) +{ +} + +CPosOriInverse::~CPosOriInverse () +{ +} + +dMatrix CPosOriInverse::Forward () +{ + CTransformMatrix T; + + for (unsigned int i=0; i<_jointList.size (); ++i) { + JointInfo *joint = _jointList[i]; + + dMatrix A = joint->TransformationMatrix (); + + joint->a = T.GetRotation (joint->axis); + joint->p = T.GetPosition (); + + T *= A; + } + _current = T.GetPositionOrientation (); + + return T; +} + +dMatrix CPosOriInverse::GetEulerVel2AngularVel (double phi, double theta, double psi) +{ + double cpsi = cos(psi); + double spsi = sin(psi); + double cthe = cos(theta); + double sthe = sin(theta); + + dMatrix C(3,3); + C(0,0) = cpsi*cthe; C(0,1) = -spsi; C(0,2) = 0; + C(1,0) = spsi*cthe; C(1,1) = cpsi; C(1,2) = 0; + C(2,0) = -sthe; C(2,1) = 0; C(2,2) = 1; + + return C; +} + +dMatrix CPosOriInverse::Jacobian() +{ + //Position Jacobian + dMatrix J(6, _jointList.size()); + J.zero (); + + dVector currentPos (3); + currentPos[0] = _current[0]; + currentPos[1] = _current[1]; + currentPos[2] = _current[2]; + + dMatrix Cinv = !GetEulerVel2AngularVel (_current[3], _current[4], _current[5]); + + for (unsigned int i=0; i<_jointList.size (); ++i) { + JointInfo *joint = _jointList[i]; + + if (joint->type == REVOLUTE_JOINT) { + if (_inverseAxis & POSITION_ONLY) { + //Position Jacobian + dVector pos = Cross (joint->a, currentPos - joint->p); + J(0,i) = pos[0]; + J(1,i) = pos[1]; + J(2,i) = pos[2]; + } + if (_inverseAxis & ORIENTATION_ONLY) { + dMatrix o = Cinv*Matrix3x1(joint->a[0], joint->a[1], joint->a[2]); + //Orientation Jacobian + J(3,i) = o(0,0); + J(4,i) = o(1,0); + J(5,i) = o(2,0); + } + } + else if (joint->type == PRISMATIC_JOINT) { + if (_inverseAxis & POSITION_ONLY) { + //Position Jacobian + J(0,i) = joint->a[0]; + J(1,i) = joint->a[1]; + J(2,i) = joint->a[2]; + } + if (_inverseAxis & ORIENTATION_ONLY) { + //Orientation Jacobian + J(3,i) = 0.; + J(4,i) = 0.; + J(5,i) = 0.; + } + } + } + return J; +} + +dVector CPosOriInverse::Error () +{ + dVector error(6); + error.zero (); + + if (_inverseAxis & POSITION_ONLY) { + error[0] = _desired[0] - _current[0]; + error[1] = _desired[1] - _current[1]; + error[2] = _desired[2] - _current[2]; + } + + if (_inverseAxis & ORIENTATION_ONLY) { + error[3] = DeltaRad (_desired[3], _current[3]); + error[4] = DeltaRad (_desired[4], _current[4]); + error[5] = DeltaRad (_desired[5], _current[5]); + } + + return error; +} + +void CPosOriInverse::SetDesired (double x, double y, double z, double phi, double theta, double psi) +{ + _desired[0] = x; + _desired[1] = y; + _desired[2] = z; + _desired[3] = DeltaRad (phi, 0); + _desired[4] = DeltaRad (theta, 0); + _desired[5] = DeltaRad (psi, 0); +} diff --git a/PhantomX-Robot-Arm-Kit/kinematics/posOriInverse.h b/PhantomX-Robot-Arm-Kit/kinematics/posOriInverse.h new file mode 100644 index 0000000..279eb27 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/kinematics/posOriInverse.h @@ -0,0 +1,34 @@ +////////////////////////////////////////////////////// +// Kinematics files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#pragma once + +#include "kinematics.h" + +#define POSITION_ONLY 1 +#define ORIENTATION_ONLY 2 +#define POSITION_ORIENTATION (POSITION_ONLY|ORIENTATION_ONLY) + +class CPosOriInverse : public CKinematics +{ +public: + CPosOriInverse (int inverseAxis = POSITION_ORIENTATION); + ~CPosOriInverse (); + + void SetDesired (double x, double y, double z, double phi, double theta, double psi); + + virtual dMatrix Forward (); + virtual dVector Error (); + virtual dMatrix Jacobian (); + +private: + dMatrix GetEulerVel2AngularVel (double phi, double theta, double psi); + + int _inverseAxis; +}; diff --git a/PhantomX-Robot-Arm-Kit/kinematics/quaternion.cpp b/PhantomX-Robot-Arm-Kit/kinematics/quaternion.cpp new file mode 100644 index 0000000..7146771 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/kinematics/quaternion.cpp @@ -0,0 +1,169 @@ +////////////////////////////////////////////////////// +// Quaternion files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#include "quaternion.h" +#include + +#define EPSILON 0.0000001 + +Quaternion::Quaternion() +: _s(1.), _v(3) +{ + _v[0] = 0.; + _v[1] = 0.; + _v[2] = 0.; +} + +Quaternion::Quaternion(const Quaternion &q) +{ + _s = q.s(); + _v = q.v(); +} + +Quaternion::Quaternion(const double angle, const dVector & axis) +{ + if(axis.size() != 3) { + return; + } + + // make sure axis is a unit vector + _v = sin(angle/2) * axis/Norm2(axis); + _s = cos(angle/2); +} + +Quaternion::Quaternion(const double s_in, const double v1, const double v2, const double v3) +: _s(s_in), _v(3) +{ + _v[0] = v1; + _v[1] = v2; + _v[2] = v3; +} + +inline double sgn (double a) +{ + return (0. <= a) ? 1. : -1; +} + +Quaternion::Quaternion(double phi, double theta, double psi) +: _s(0.), _v(3) +{ + Quaternion Qx (cos(phi/2), sin(phi/2), 0, 0 ); + Quaternion Qy (cos(theta/2), 0, sin(theta/2), 0 ); + Quaternion Qz (cos(psi/2), 0, 0, sin(psi/2)); + + /* + *this = Qx*Qy*Qz; + */ + + *this = Qz*Qy*Qx; +} + +Quaternion Quaternion::operator+(const Quaternion & rhs)const +{ + Quaternion q; + q._s = _s + rhs._s; + q._v = _v + rhs._v; + + return q; +} + +Quaternion Quaternion::operator-(const Quaternion & rhs)const +{ + Quaternion q; + q._s = _s - rhs._s; + q._v = _v - rhs._v; + + return q; +} + +Quaternion Quaternion::operator*(const Quaternion & rhs)const +{ + Quaternion q; + q._s = _s * rhs._s - Dot (_v, rhs._v); + q._v = _s * rhs._v + rhs._s * _v + Cross (_v, rhs._v); + + return q; +} + + +Quaternion Quaternion::operator/(const Quaternion & rhs)const +{ + return *this*rhs.i(); +} + + +Quaternion Quaternion::conjugate()const +{ + Quaternion q; + q._s = _s; + q._v = -1.*_v; + + return q; +} + +double Quaternion::norm()const +{ + return( sqrt(_s*_s + Dot (_v, _v)) ); +} + +Quaternion & Quaternion::unit() +{ + double tmp = norm(); + if(tmp > EPSILON) { + _s = _s/tmp; + _v = _v/tmp; + } + return *this; +} + +Quaternion Quaternion::i()const +{ + return conjugate()/norm(); +} + +double Quaternion::dot(const Quaternion & q)const +{ + return (_s*q._s + _v[0]*q._v[0] + _v[1]*q._v[1] + _v[2]*q._v[2]); +} + +void Quaternion::set_v(const dVector & v) +{ + if(v.size() == 3) { + _v = v; + } + else { + return; + } +} + +Quaternion operator*(const double c, const Quaternion & q) +{ + Quaternion out; + out.set_s(q.s() * c); + out.set_v(q.v() * c); + return out; +} + +Quaternion operator*(const Quaternion & q, const double c) +{ + return operator*(c, q); +} + +Quaternion operator/(const double c, const Quaternion & q) +{ + Quaternion out; + out.set_s(q.s() / c); + out.set_v(q.v() / c); + return out; +} + +Quaternion operator/(const Quaternion & q, const double c) +{ + return operator/(c, q); +} diff --git a/PhantomX-Robot-Arm-Kit/kinematics/quaternion.h b/PhantomX-Robot-Arm-Kit/kinematics/quaternion.h new file mode 100644 index 0000000..511bc73 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/kinematics/quaternion.h @@ -0,0 +1,56 @@ +////////////////////////////////////////////////////// +// Quaternion files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#pragma once + +#include "matrix/matrixAlgebra.h" + +class Quaternion +{ +public: + Quaternion(); + Quaternion(const Quaternion &q); + Quaternion(const double angle_in_rad, const dVector & axis); + Quaternion(const double s, const double v1, const double v2, const double v3); + Quaternion(double phi, double theta, double psi); + + Quaternion operator+(const Quaternion & q)const; + Quaternion operator-(const Quaternion & q)const; + Quaternion operator*(const Quaternion & q)const; + Quaternion operator/(const Quaternion & q)const; + + Quaternion conjugate()const; + double norm()const; + Quaternion &unit(); + Quaternion i()const; + double dot(const Quaternion & q)const; + + double s() const { return _s; } // Return scalar part. + void set_s (const double s){ _s = s; } // Set scalar part. + dVector v() const { return _v; } // Return vector part. + void set_v (const dVector & v); // Set vector part. + + Quaternion & operator = (const Quaternion &rhs) + { + if (this != &rhs) { + _s = rhs.s(); + _v = rhs.v(); + } + return *this; + } + +private: + double _s; // Quaternion scalar part. + dVector _v; // Quaternion vector part. +}; + +Quaternion operator * (const double c, const Quaternion & rhs); +Quaternion operator * (const Quaternion & lhs, const double c); +Quaternion operator / (const double c, const Quaternion & rhs); +Quaternion operator / (const Quaternion & lhs, const double c); diff --git a/PhantomX-Robot-Arm-Kit/main.cpp b/PhantomX-Robot-Arm-Kit/main.cpp new file mode 100644 index 0000000..b48f94e --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/main.cpp @@ -0,0 +1,11 @@ +#include "mainwindow.h" +#include + +int main(int argc, char *argv[]) +{ + QApplication a(argc, argv); + MainWindow w; + w.show(); + + return a.exec(); +} diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.cpp b/PhantomX-Robot-Arm-Kit/mainwindow.cpp new file mode 100644 index 0000000..ed247d7 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/mainwindow.cpp @@ -0,0 +1,84 @@ +#include "mainwindow.h" +#include "ui_mainwindow.h" + +MainWindow::MainWindow(QWidget *parent) :QMainWindow(parent), ui(new Ui::MainWindow) { + + ui->setupUi(this); + + _slider = new QSlider*[6]; + _slider[0] = ui->horizontalSlider0; + _slider[1] = ui->horizontalSlider1; + _slider[2] = ui->horizontalSlider2; + _slider[3] = ui->horizontalSlider3; + _slider[4] = ui->horizontalSlider4; + _slider[5] = ui->horizontalSlider5; + + _lineEdit = new QLineEdit*[6]; + _lineEdit[0] = ui->lineEdit0; + _lineEdit[1] = ui->lineEdit1; + _lineEdit[2] = ui->lineEdit2; + _lineEdit[3] = ui->lineEdit3; + _lineEdit[4] = ui->lineEdit4; + _lineEdit[5] = ui->lineEdit5; + + for(int i=0; i<6; i++) { + + // set range, only number + _lineEdit[i]->setValidator( new QIntValidator(-150, 150, this) ); + + // set text signal + connect(_lineEdit[i], &QLineEdit::textChanged, [=](QString text) { bool ok; _slider[i]->setValue(text.toInt(&ok));}); + + // set slider signal + connect(_slider[i], &QSlider::valueChanged, [=](int value) { valueChanged(i,value); }); + } + + // set link, joint + _kinematics = new CPosOriInverse(POSITION_ORIENTATION); + _kinematics->AttachJoint(REVOLUTE_JOINT, 2, 0.0, 0.0, 0.000, -90 * _DEG2RAD, 0 * _DEG2RAD, 0 * _DEG2RAD, 0.01, -180.0 * _DEG2RAD, 180.0 * _DEG2RAD, 0 * _DEG2RAD, 1.); + _kinematics->AttachJoint(REVOLUTE_JOINT, 0, 0.0, 0.0, 0.150, 0 * _DEG2RAD, 0 * _DEG2RAD, 0 * _DEG2RAD, 0.01, -180.0 * _DEG2RAD, 180.0 * _DEG2RAD, 0 * _DEG2RAD, 1.); + _kinematics->AttachJoint(REVOLUTE_JOINT, 0, 0.0, 0.0, 0.145, 0 * _DEG2RAD, 0 * _DEG2RAD, 0 * _DEG2RAD, 0.01, -180.0 * _DEG2RAD, 180.0 * _DEG2RAD, 0 * _DEG2RAD, 1.); + _kinematics->AttachJoint(REVOLUTE_JOINT, 0, 0.0, 0.0, 0.070, 0 * _DEG2RAD, 0 * _DEG2RAD, 0 * _DEG2RAD, 0.01, -180.0 * _DEG2RAD, 180.0 * _DEG2RAD, 0 * _DEG2RAD, 1.); + _kinematics->AttachJoint(REVOLUTE_JOINT, 2, 0.0, 0.0, 0.080, 0 * _DEG2RAD, 0 * _DEG2RAD, 0 * _DEG2RAD, 0.01, -180.0 * _DEG2RAD, 180.0 * _DEG2RAD, 0 * _DEG2RAD, 1.); + _kinematics->AttachJoint(PRISMATIC_JOINT, 0, 0.0, 0.0, 0.0, 0 * _DEG2RAD, 0 * _DEG2RAD, 0 * _DEG2RAD, 0.01, -180.0 * _DEG2RAD, 180.0 * _DEG2RAD, 0 * _DEG2RAD, 1.); + ui->widget->setKinematics(_kinematics); + + // set init position + _qDefault = dVector(6); + _qDefault[0] = 0.0; + _qDefault[1] = 90.0; + _qDefault[2] = -90.0; + _qDefault[3] = 0.0; + _qDefault[4] = 0.0; + _qDefault[5] = 0.0; + + _q = _qDefault * _DEG2RAD; + + ui->widget->setJointAngle(_q); +} + +MainWindow::~MainWindow() { + delete ui; +} + +void MainWindow::valueChanged(int index, int val) { + + _q[index] = val * _DEG2RAD; + ui->widget->setJointAngle(_q); + ui->widget->updateGL(); + + _lineEdit[index]->setText(QString::number(val)); +} + +void MainWindow::on_buttonReset_clicked() { + + _q = _qDefault * _DEG2RAD; + + ui->widget->setJointAngle(_q); + ui->widget->updateGL(); + + for(int i=0; i<6; i++) { + _lineEdit[i]->setText(QString::number(_qDefault[i])); + _slider[i]->setValue(_qDefault[i]); + } +} diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.h b/PhantomX-Robot-Arm-Kit/mainwindow.h new file mode 100644 index 0000000..20ea7ac --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/mainwindow.h @@ -0,0 +1,37 @@ +#ifndef MAINWINDOW_H +#define MAINWINDOW_H + +#include +#include +#include + +#include "kinematics/kinematics.h" + +namespace Ui { +class MainWindow; +} + +class MainWindow : public QMainWindow +{ + Q_OBJECT + +public: + explicit MainWindow(QWidget *parent = 0); + ~MainWindow(); + +private: + Ui::MainWindow *ui; + QSlider **_slider; + QLineEdit **_lineEdit; + + CKinematics *_kinematics; + dVector _qDefault; + dVector _q; + +public slots: + void valueChanged(int index, int val); +private slots: + void on_buttonReset_clicked(); +}; + +#endif // MAINWINDOW_H diff --git a/PhantomX-Robot-Arm-Kit/mainwindow.ui b/PhantomX-Robot-Arm-Kit/mainwindow.ui new file mode 100644 index 0000000..b172f50 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/mainwindow.ui @@ -0,0 +1,436 @@ + + + MainWindow + + + + 0 + 0 + 800 + 405 + + + + PhantomX Robot Arm Kit + + + + :/icon/image/phantomX-robot-arm-kit.png:/icon/image/phantomX-robot-arm-kit.png + + + + + + + + 0 + 0 + + + + + + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + -90 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 200 + 0 + + + + -150 + + + 150 + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + -150 + + + 150 + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + -150 + + + 150 + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + -150 + + + 150 + + + Qt::Horizontal + + + + + + + 5 + + + Qt::AlignCenter + + + + + + + 2 + + + Qt::AlignCenter + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + Quit + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + -150 + + + 150 + + + -90 + + + Qt::Horizontal + + + + + + + 4 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + -150 + + + 150 + + + 90 + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + 90 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 80 + 16777215 + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 1 + + + Qt::AlignCenter + + + + + + + 3 + + + Qt::AlignCenter + + + + + + + Reset + + + + + + + + + + + + + + GLWidget + QWidget +
glwidget.h
+ 1 + + + + + + + + pushButton + clicked() + MainWindow + close() + + + 911 + 412 + + + 597 + 220 + + + + + diff --git a/PhantomX-Robot-Arm-Kit/matrix/mat/matrix.h b/PhantomX-Robot-Arm-Kit/matrix/mat/matrix.h new file mode 100644 index 0000000..6cbd4e9 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/matrix/mat/matrix.h @@ -0,0 +1,904 @@ +//////////////////////////////// +// Matrix TCL Lite v1.13 +// Copyright (c) 1997-2002 Techsoft Pvt. Ltd. (See License.Txt file.) +// +// Matrix.h: Matrix C++ template class include file +// Web: http://www.techsoftpl.com/matrix/ +// Email: matrix@techsoftpl.com +// + +#if !defined(__STD_MATRIX_H) +#define __STD_MATRIX_H + +#include +#include +#include +#include +#include +#include + +using namespace std; + +inline void _matrix_error (const char* pErrMsg) +{ + cout << pErrMsg << endl; + exit(1); +} + +template +class matrix +{ +public: + // Constructors + matrix (const matrix& m); + matrix (size_t row = 1, size_t col = 1); + + // Destructor + ~matrix (); + + // Assignment operators + matrix& operator = (const matrix& m); + + // Value extraction method + size_t RowNo () const { return _m->Row; } + size_t ColNo () const { return _m->Col; } + + // Subscript operator + T& operator () (size_t row, size_t col); + T operator () (size_t row, size_t col) const; + + // Unary operators + matrix operator + () { return *this; } + matrix operator - (); + + // Combined assignment - calculation operators + matrix& operator += (const matrix& m); + matrix& operator -= (const matrix& m); + matrix& operator *= (const matrix& m); + matrix& operator *= (const T& c); + matrix& operator /= (const T& c); + matrix& operator ^= (const size_t& pow); + + // Miscellaneous -methods + void Null (const size_t& row, const size_t& col); + void Null (); + void Unit (const size_t& row); + void Unit (); + void SetSize (size_t row, size_t col); + + // Utility methods + matrix Solve (const matrix& v) const; + matrix Adj (); + matrix Inv (); + T Det () const; + T Norm (); + T Cofact (size_t row, size_t col); + T Cond (); + + // Type of matrices + bool IsSquare () { return (_m->Row == _m->Col); } + bool IsSingular (); + bool IsDiagonal (); + bool IsScalar (); + bool IsUnit (); + bool IsNull (); + bool IsSymmetric (); + bool IsSkewSymmetric (); + bool IsUpperTriangular (); + bool IsLowerTriangular (); + +private: + struct base_mat { + T **Val; + T *_Val; // page365: ޸ ϵ ϱ ؼ ߰ + size_t Row, Col, RowSiz, ColSiz; + int Refcnt; + + base_mat (size_t row, size_t col, T** v) + { + Row = row; RowSiz = row; + Col = col; ColSiz = col; + Refcnt = 1; + + Val = new T* [row]; + _Val = new T [row*col]; + size_t rowlen = col * sizeof(T); + + for (size_t i=0; i < row; i++) + { + // Val[i] = new T [col]; + Val[i] = &_Val[i*col]; + if (v) memcpy( Val[i], v[i], rowlen); + } + } + ~base_mat () + { + //for (size_t i=0; i < RowSiz; i++) + // delete [] Val[i]; + delete [] Val; + delete [] _Val; + } + }; + base_mat *_m; + + void clone (); + void realloc (size_t row, size_t col); + int pivot (size_t row); +}; + +// constructor +template inline +matrix::matrix (size_t row, size_t col) +{ + _m = new base_mat( row, col, 0); +} + +// copy constructor +template inline +matrix::matrix (const matrix& m) +{ + _m = m._m; + _m->Refcnt++; +} + +// Internal copy constructor +template inline void +matrix::clone () +{ + _m->Refcnt--; + _m = new base_mat( _m->Row, _m->Col, _m->Val); +} + +// destructor +template inline +matrix::~matrix () +{ + if (--_m->Refcnt == 0) delete _m; +} + +// assignment operator +template inline matrix& +matrix::operator = (const matrix& m) +{ + m._m->Refcnt++; + if (--_m->Refcnt == 0) delete _m; + _m = m._m; + return *this; +} + +// reallocation method +template inline void +matrix::realloc (size_t row, size_t col) +{ + if (row == _m->RowSiz && col == _m->ColSiz) + { + _m->Row = _m->RowSiz; + _m->Col = _m->ColSiz; + return; + } + + base_mat *m1 = new base_mat( row, col, NULL); + size_t colSize = min(_m->Col,col) * sizeof(T); + size_t minRow = min(_m->Row,row); + + for (size_t i=0; i < minRow; i++) + memcpy( m1->Val[i], _m->Val[i], colSize); + + if (--_m->Refcnt == 0) + delete _m; + _m = m1; + + return; +} + +// public method for resizing matrix +template inline void +matrix::SetSize (size_t row, size_t col) +{ + size_t i,j; + size_t oldRow = _m->Row; + size_t oldCol = _m->Col; + + if (row != _m->RowSiz || col != _m->ColSiz) + realloc( row, col); + + for (i=oldRow; i < row; i++) + for (j=0; j < col; j++) + _m->Val[i][j] = T(0); + + for (i=0; i < row; i++) + for (j=oldCol; j < col; j++) + _m->Val[i][j] = T(0); + + return; +} + +// subscript operator to get/set individual elements +template inline T& +matrix::operator () (size_t row, size_t col) +{ + if (row >= _m->Row || col >= _m->Col) + _matrix_error( "matrix::operator(): Index out of range!"); + + if (_m->Refcnt > 1) clone(); + + return _m->Val[row][col]; +} + +// subscript operator to get/set individual elements +template inline T +matrix::operator () (size_t row, size_t col) const +{ + if (row >= _m->Row || col >= _m->Col) + _matrix_error( "matrix::operator(): Index out of range!"); + + return _m->Val[row][col]; +} + +// input stream function +template inline istream& +operator >> (istream& istrm, matrix& m) +{ + for (size_t i=0; i < m.RowNo(); i++) + for (size_t j=0; j < m.ColNo(); j++) + { + T x; + istrm >> x; + m(i,j) = x; + } + + return istrm; +} + +// output stream function +template inline ostream& +operator << (ostream& ostrm, const matrix& m) +{ + for (size_t i=0; i < m.RowNo(); i++) + { + for (size_t j=0; j < m.ColNo(); j++) + { + T x = m(i,j); + ostrm << x << '\t'; + } + ostrm << endl; + } + return ostrm; +} + + +// logical equal-to operator +template inline bool +operator == (const matrix& m1, const matrix& m2) +{ + if (m1.RowNo() != m2.RowNo() || m1.ColNo() != m2.ColNo()) + return false; + + for (size_t i=0; i < m1.RowNo(); i++) + for (size_t j=0; j < m1.ColNo(); j++) + if (m1(i,j) != m2(i,j)) + return false; + + return true; +} + +// logical no-equal-to operator +template inline bool +operator != (const matrix& m1, const matrix& m2) +{ + return (m1 == m2) ? false : true; +} + +// combined addition and assignment operator +template inline matrix& +matrix::operator += (const matrix& m) +{ + if (_m->Row != m._m->Row || _m->Col != m._m->Col) + _matrix_error( "matrix::operator+= : Inconsistent matrix sizes in addition!"); + if (_m->Refcnt > 1) clone(); + for (size_t i=0; i < m._m->Row; i++) + for (size_t j=0; j < m._m->Col; j++) + _m->Val[i][j] += m._m->Val[i][j]; + + return *this; +} + +// combined subtraction and assignment operator +template inline matrix& +matrix::operator -= (const matrix& m) +{ + if (_m->Row != m._m->Row || _m->Col != m._m->Col) + _matrix_error( "matrix::operator-= : Inconsistent matrix sizes in subtraction!"); + if (_m->Refcnt > 1) clone(); + for (size_t i=0; i < m._m->Row; i++) + for (size_t j=0; j < m._m->Col; j++) + _m->Val[i][j] -= m._m->Val[i][j]; + + return *this; +} + +// combined scalar multiplication and assignment operator +template inline matrix& +matrix::operator *= (const T& c) +{ + if (_m->Refcnt > 1) clone(); + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + _m->Val[i][j] *= c; + + return *this; +} + +// combined matrix multiplication and assignment operator +template inline matrix& +matrix::operator *= (const matrix& m) +{ + if (_m->Col != m._m->Row) + _matrix_error( "matrix::operator*= : Inconsistent matrix sizes in multiplication!"); + + matrix temp(_m->Row,m._m->Col); + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < m._m->Col; j++) + { + temp._m->Val[i][j] = T(0); + for (size_t k=0; k < _m->Col; k++) + temp._m->Val[i][j] += _m->Val[i][k] * m._m->Val[k][j]; + } + + *this = temp; + + return *this; +} + +// combined scalar division and assignment operator +template inline matrix& +matrix::operator /= (const T& c) +{ + if (_m->Refcnt > 1) clone(); + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + _m->Val[i][j] /= c; + + return *this; +} + +// combined power and assignment operator +template inline matrix& +matrix::operator ^= (const size_t& pow) +{ + matrix temp(*this); + + for (size_t i=2; i <= pow; i++) + *this = *this * temp; + + return *this; +} + +// unary negation operator +template inline matrix +matrix::operator - () +{ + matrix temp(_m->Row,_m->Col); + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + temp._m->Val[i][j] = - _m->Val[i][j]; + + return temp; +} + +// binary addition operator +template inline matrix +operator + (const matrix& m1, const matrix& m2) +{ + matrix temp = m1; + temp += m2; + return temp; +} + +// binary subtraction operator +template inline matrix +operator - (const matrix& m1, const matrix& m2) +{ + matrix temp = m1; + temp -= m2; + return temp; +} + +// binary scalar multiplication operator +template inline matrix +operator * (const matrix& m, const T& no) +{ + matrix temp = m; + temp *= no; + return temp; +} + + +// binary scalar multiplication operator +template inline matrix +operator * (const T& no, const matrix& m) +{ + return (m * no); +} + +// binary matrix multiplication operator +template inline matrix +operator * (const matrix& m1, const matrix& m2) +{ + matrix temp = m1; + temp *= m2; + return temp; +} + +// binary scalar division operator +template inline matrix +operator / (const matrix& m, const T& no) +{ + return (m * (T(1) / no)); +} + + +// binary scalar division operator +template inline matrix +operator / (const T& no, const matrix& m) +{ + return (!m * no); +} + +// binary matrix division operator +template inline matrix +operator / (const matrix& m1, const matrix& m2) +{ + return (m1 * !m2); +} + +// binary power operator +template inline matrix +operator ^ (const matrix& m, const size_t& pow) +{ + matrix temp = m; + temp ^= pow; + return temp; +} + +// unary transpose operator +template inline matrix +operator ~ (const matrix& m) +{ + matrix temp(m.ColNo(),m.RowNo()); + + for (size_t i=0; i < m.RowNo(); i++) + for (size_t j=0; j < m.ColNo(); j++) + { + T x = m(i,j); + temp(j,i) = x; + } + + return temp; +} + +// unary inversion operator +template inline matrix +operator ! (const matrix m) +{ + matrix temp = m; + return temp.Inv(); +} + +// inversion function +template inline matrix +matrix::Inv () +{ + size_t i,j,k; + T a1,a2,*rowptr; + + if (_m->Row != _m->Col) + _matrix_error( "matrix::operator!: Inversion of a non-square matrix"); + + matrix temp(_m->Row,_m->Col); + if (_m->Refcnt > 1) clone(); + + temp.Unit(); + + for (k=0; k < _m->Row; k++) + { + int indx = pivot(k); + if (indx == -1) + _matrix_error( "matrix::operator!: Inversion of a singular matrix"); + + if (indx != 0) + { + rowptr = temp._m->Val[k]; + temp._m->Val[k] = temp._m->Val[indx]; + temp._m->Val[indx] = rowptr; + } + + a1 = _m->Val[k][k]; + for (j=0; j < _m->Row; j++) + { + _m->Val[k][j] /= a1; + temp._m->Val[k][j] /= a1; + } + + for (i=0; i < _m->Row; i++) + if (i != k) + { + a2 = _m->Val[i][k]; + for (j=0; j < _m->Row; j++) + { + _m->Val[i][j] -= a2 * _m->Val[k][j]; + temp._m->Val[i][j] -= a2 * temp._m->Val[k][j]; + } + } + } + + return temp; +} + +// solve simultaneous equation +template inline matrix +matrix::Solve (const matrix& v) const +{ + size_t i,j,k; + T a1; + + if (!(_m->Row == _m->Col && _m->Col == v._m->Row)) + _matrix_error( "matrix::Solve():Inconsistent matrices!"); + + matrix temp(_m->Row,_m->Col+v._m->Col); + for (i=0; i < _m->Row; i++) + { + for (j=0; j < _m->Col; j++) + temp._m->Val[i][j] = _m->Val[i][j]; + for (k=0; k < v._m->Col; k++) + temp._m->Val[i][_m->Col+k] = v._m->Val[i][k]; + } + + for (k=0; k < _m->Row; k++) + { + int indx = temp.pivot(k); + if (indx == -1) + _matrix_error( "matrix::Solve(): Singular matrix!"); + + a1 = temp._m->Val[k][k]; + for (j=k; j < temp._m->Col; j++) + temp._m->Val[k][j] /= a1; + + for (i=k+1; i < _m->Row; i++) + { + a1 = temp._m->Val[i][k]; + for (j=k; j < temp._m->Col; j++) + temp._m->Val[i][j] -= a1 * temp._m->Val[k][j]; + } + } + + matrix s(v._m->Row,v._m->Col); + for (k=0; k < v._m->Col; k++) + for (int m=int(_m->Row)-1; m >= 0; m--) + { + s._m->Val[m][k] = temp._m->Val[m][_m->Col+k]; + for (j=m+1; j < _m->Col; j++) + s._m->Val[m][k] -= temp._m->Val[m][j] * s._m->Val[j][k]; + } + + return s; +} + +// set zero to all elements of this matrix +template inline void +matrix::Null (const size_t& row, const size_t& col) +{ + if (row != _m->Row || col != _m->Col) + realloc( row,col); + + if (_m->Refcnt > 1) + clone(); + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + _m->Val[i][j] = T(0); + + return; +} + +// set zero to all elements of this matrix +template inline void +matrix::Null() +{ + if (_m->Refcnt > 1) clone(); + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + _m->Val[i][j] = T(0); + + return; +} + +// set this matrix to unity +template inline void +matrix::Unit (const size_t& row) +{ + if (row != _m->Row || row != _m->Col) + realloc( row, row); + + if (_m->Refcnt > 1) + clone(); + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + _m->Val[i][j] = i == j ? T(1) : T(0); + + return; +} + +// set this matrix to unity +template inline void +matrix::Unit () +{ + if (_m->Refcnt > 1) clone(); + size_t row = min(_m->Row,_m->Col); + _m->Row = _m->Col = row; + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + _m->Val[i][j] = i == j ? T(1) : T(0); + + return; +} + +// private partial pivoting method +template inline int +matrix::pivot (size_t row) +{ + int k = int(row); + double amax,temp; + + amax = -1; + for (size_t i=row; i < _m->Row; i++) + if ( (temp = abs( _m->Val[i][row])) > amax && temp != 0.0) + { + amax = temp; + k = i; + } + if (_m->Val[k][row] == T(0)) + return -1; + + if (k != int(row)) + { + T* rowptr = _m->Val[k]; + _m->Val[k] = _m->Val[row]; + _m->Val[row] = rowptr; + return k; + } + return 0; +} + +// calculate the determinant of a matrix +template inline T +matrix::Det () const +{ + size_t i,j,k; + T piv,detVal = T(1); + + if (_m->Row != _m->Col) + _matrix_error( "matrix::Det(): Determinant a non-square matrix!"); + + matrix temp(*this); + if (temp._m->Refcnt > 1) temp.clone(); + + for (k=0; k < _m->Row; k++) + { + int indx = temp.pivot(k); + if (indx == -1) + return 0; + if (indx != 0) + detVal = - detVal; + detVal = detVal * temp._m->Val[k][k]; + for (i=k+1; i < _m->Row; i++) + { + piv = temp._m->Val[i][k] / temp._m->Val[k][k]; + for (j=k+1; j < _m->Row; j++) + temp._m->Val[i][j] -= piv * temp._m->Val[k][j]; + } + } + return detVal; +} + +// calculate the norm of a matrix +template inline T +matrix::Norm () +{ + T retVal = T(0); + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + retVal += _m->Val[i][j] * _m->Val[i][j]; + + retVal = sqrt( retVal); + + return retVal; +} + +// calculate the condition number of a matrix +template inline T +matrix::Cond () +{ + matrix inv = ! (*this); + return (Norm() * inv.Norm()); +} + +// calculate the cofactor of a matrix for a given element +template inline T +matrix::Cofact (size_t row, size_t col) +{ + size_t i,i1,j,j1; + + if (_m->Row != _m->Col) + _matrix_error( "matrix::Cofact(): Cofactor of a non-square matrix!"); + + if (row > _m->Row || col > _m->Col) + _matrix_error( "matrix::Cofact(): Index out of range!"); + + matrix temp (_m->Row-1,_m->Col-1); + + for (i=i1=0; i < _m->Row; i++) + { + if (i == row) + continue; + for (j=j1=0; j < _m->Col; j++) + { + if (j == col) + continue; + temp._m->Val[i1][j1] = _m->Val[i][j]; + j1++; + } + i1++; + } + + T cof = temp.Det(); + if ((row+col)%2 == 1) + cof = -cof; + + return cof; +} + + +// calculate adjoin of a matrix +template inline matrix +matrix::Adj () +{ + if (_m->Row != _m->Col) + _matrix_error( "matrix::Adj(): Adjoin of a non-square matrix."); + + matrix temp(_m->Row,_m->Col); + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + temp._m->Val[j][i] = Cofact(i,j); + + return temp; +} + +// Determine if the matrix is singular +template inline bool +matrix::IsSingular () +{ + if (_m->Row != _m->Col) + return false; + + return (Det() == T(0)); +} + +// Determine if the matrix is diagonal +template inline bool +matrix::IsDiagonal () +{ + if (_m->Row != _m->Col) + return false; + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + if (i != j && _m->Val[i][j] != T(0)) + return false; + + return true; +} + +// Determine if the matrix is scalar +template inline bool +matrix::IsScalar () +{ + if (!IsDiagonal()) + return false; + + T v = _m->Val[0][0]; + for (size_t i=1; i < _m->Row; i++) + if (_m->Val[i][i] != v) + return false; + + return true; +} + +// Determine if the matrix is a unit matrix +template inline bool +matrix::IsUnit () +{ + if (IsScalar() && _m->Val[0][0] == T(1)) + return true; + + return false; +} + +// Determine if this is a null matrix +template inline bool +matrix::IsNull () +{ + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + if (_m->Val[i][j] != T(0)) + return false; + + return true; +} + +// Determine if the matrix is symmetric +template inline bool +matrix::IsSymmetric () +{ + if (_m->Row != _m->Col) + return false; + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + if (_m->Val[i][j] != _m->Val[j][i]) + return false; + + return true; +} + +// Determine if the matrix is skew-symmetric +template inline bool +matrix::IsSkewSymmetric () +{ + if (_m->Row != _m->Col) + return false; + + for (size_t i=0; i < _m->Row; i++) + for (size_t j=0; j < _m->Col; j++) + if (_m->Val[i][j] != -_m->Val[j][i]) + return false; + + return true; +} + +// Determine if the matrix is upper triangular +template inline bool +matrix::IsUpperTriangular () +{ + if (_m->Row != _m->Col) + return false; + for (size_t i=1; i < _m->Row; i++) + for (size_t j=0; j < i-1; j++) + if (_m->Val[i][j] != T(0)) + return false; + + return true; +} + +// Determine if the matrix is lower triangular +template inline bool +matrix::IsLowerTriangular () +{ + if (_m->Row != _m->Col) + return false; + + for (size_t j=1; j < _m->Col; j++) + for (size_t i=0; i < j-1; i++) + if (_m->Val[i][j] != T(0)) + return false; + + return true; +} + +#endif //__STD_MATRIX_H diff --git a/PhantomX-Robot-Arm-Kit/matrix/mathDef.h b/PhantomX-Robot-Arm-Kit/matrix/mathDef.h new file mode 100644 index 0000000..0cd2ee0 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/matrix/mathDef.h @@ -0,0 +1,48 @@ +////////////////////////////////////////////////////// +// Math files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// +// Collection of Mathematical functions + +#pragma once +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 // pi +#endif + +#define _RAD2DEG (180./M_PI) +#define _DEG2RAD (M_PI/180.) + + +// a ȣ Ѵ. +inline double Sign (double a) +{ + return (0. < a) ? 1. : ((a < 0) ? -1. : 0.); +} + +// ang1 ang2 հ Ͽ Ѵ. +// -pi ~ pi ̴. +inline double DeltaRad (double ang1, double ang2) +{ + double da = ang1 - ang2; + if (-M_PI < da && da < M_PI) return da; + else { + da = fmod (da, 2*M_PI); + if (M_PI <= da) return da - 2*M_PI; + else if (da <= -M_PI) return da + 2*M_PI; + else return da; + } + return da; +} + +inline double Limit (double x, double lo, double hi) +{ + if (x < lo) return lo; + if (x > hi) return hi; + return x; +} \ No newline at end of file diff --git a/PhantomX-Robot-Arm-Kit/matrix/matrixAlgebra.h b/PhantomX-Robot-Arm-Kit/matrix/matrixAlgebra.h new file mode 100644 index 0000000..8f4706a --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/matrix/matrixAlgebra.h @@ -0,0 +1,147 @@ +////////////////////////////////////////////////////// +// Vector and Matrix files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// +// Collection of Mathematical functions + +#pragma once + +#include "mat/matrix.h" + +class dMatrix : public matrix +{ +public: + dMatrix (const dMatrix &m): matrix (m) { } + dMatrix (const matrix &m): matrix (m) { } + dMatrix (size_t row = 1, size_t col = 1): matrix (row, col) { } + + size_t rowno () const { return RowNo(); } + size_t colno () const { return ColNo(); } + + size_t size() const { return RowNo()*ColNo(); } + void resize(size_t row, size_t col) { SetSize (row, col); } + + void unit () { Unit (); } + void zero () { Null (); } + + dMatrix operator = (const dMatrix &m) + { + if (this != &m) { + matrix::operator = (m); + } + return *this; + } +}; + +class dVector : public matrix +{ +public: + dVector (const dVector &m): matrix (m) { } + dVector (const matrix &m): matrix (m) { } + dVector (size_t size = 3): matrix (size, 1) { } + + size_t size() const { return RowNo()*ColNo(); } + void resize(size_t size) { SetSize (size, 1); } + + void zero () { Null (); } + + double operator[] (size_t i) const + { + return (*this)(i,0); + } + + double& operator[] (size_t i) + { + return (*this)(i,0); + } + + dVector operator = (const dVector &m) + { + if (this != &m) { + matrix::operator = (m); + } + return *this; + } +}; + + +// dVector (dot product) Ѵ. +inline double Dot (const dVector &v1, const dVector &v2) +{ + double v = 0.; + for(unsigned int i=0; i +// + +#include "transformation.h" + +CTransformMatrix::CTransformMatrix () +: dMatrix (4, 4) +{ + unit (); +} + +CTransformMatrix::CTransformMatrix (double x, double y, double z, double phi, double theta, double psi) +: dMatrix (4, 4) +{ + dMatrix &A = *this; + + double sphi = sin(phi), cphi = cos(phi); + double stht = sin(theta), ctht = cos(theta); + double spsi = sin(psi), cpsi = cos(psi); + + // transformation matrix: Trans(x,y,z) x Rz(psi) x Ry(theta) x Rx(phi) + // ǥ ȯ : + // 1. x, y, z ŭ ġ ̵Ѵ. + // 2. z psi ŭ ȸѴ. + // 3. y theta ŭ ȸѴ. + // 4. x phi ŭ ȸѴ. + A(0,0) = cpsi*ctht; A(0,1) = cpsi*stht*sphi - spsi*cphi; A(0,2) = cpsi*stht*cphi + spsi*sphi; A(0,3) = x; + A(1,0) = spsi*ctht; A(1,1) = spsi*stht*sphi + cpsi*cphi; A(1,2) = spsi*stht*cphi - cpsi*sphi; A(1,3) = y; + A(2,0) = -stht; A(2,1) = ctht*sphi; A(2,2) = ctht*cphi; A(2,3) = z; + A(3,0) = 0; A(3,1) = 0; A(3,2) = 0; A(3,3) = 1; + + /* + // transformation matrix: Trans(x,y,z) x Rx(phi) x Ry(theta) x Rz(psi) + // ǥ ȯ : + // 1. x, y, z ŭ ġ ̵Ѵ. + // 2. x phi ŭ ȸѴ. + // 3. y theta ŭ ȸѴ. + // 4. z psi ŭ ȸѴ. + A(0,0) = ctht*cpsi; A(0,1) = -ctht*spsi; A(0,2) = stht; A(0,3) = x; + A(1,0) = sphi*stht*cpsi + cphi*spsi; A(1,1) = -sphi*stht*spsi + cphi*cpsi; A(1,2) = -sphi*ctht; A(1,3) = y; + A(2,0) = -cphi*stht*cpsi + sphi*spsi; A(2,1) = cphi*stht*spsi + sphi*cpsi; A(2,2) = cphi*ctht; A(2,3) = z; + A(3,0) = 0; A(3,1) = 0; A(3,2) = 0; A(3,3) = 1; + */ +} + +dVector CTransformMatrix::GetRotation (int axis) const +{ + const dMatrix &A = *this; + dVector v(3); + + v[0] = A(0,axis); + v[1] = A(1,axis); + v[2] = A(2,axis); + + return v; +} + +dVector CTransformMatrix::GetPosition () const +{ + const dMatrix &A = *this; + dVector v(3); + + v[0] = A(0,3); + v[1] = A(1,3); + v[2] = A(2,3); + + return v; +} + +dVector CTransformMatrix::GetOrientation () const +{ + const dMatrix &A = *this; + dVector v(3); + + /* + v[0] = atan2(-A(1,2), A(2,2)); + v[1] = atan2( A(0,2), sqrt(A(1,2)*A(1,2) + A(2,2)*A(2,2)) ); + v[2] = atan2(-A(0,1), A(0,0)); + */ + v[0] = atan2( A(2,1), A(2,2)); + v[1] = atan2(-A(2,0), sqrt(A(2,1)*A(2,1) + A(2,2)*A(2,2)) ); + v[2] = atan2( A(1,0), A(0,0)); + + return v; +} + +dVector CTransformMatrix::GetOrientation2 () const +{ + const dMatrix &A = *this; + dVector v(3); + + // theta in the range (pi/2, 3pi/2) + /* + v[0] = atan2( A(1,2), -A(2,2)); + v[1] = atan2( A(0,2), -sqrt(A(1,2)*A(1,2) + A(2,2)*A(2,2)) ); + v[2] = atan2( A(0,1), -A(0,0)); + */ + v[0] = atan2(-A(2,1),-A(2,2)); + v[1] = atan2(-A(2,0),-sqrt(A(2,1)*A(2,1) + A(2,2)*A(2,2)) ); + v[2] = atan2(-A(1,0),-A(0,0)); + + return v; +} + +dVector CTransformMatrix::GetPositionOrientation () const +{ + const dMatrix &A = *this; + dVector v(6); + + v[0] = A(0,3); + v[1] = A(1,3); + v[2] = A(2,3); + // theta in the range (-pi/2, pi/2) + /* + v[3] = atan2(-A(1,2), A(2,2)); + v[4] = atan2( A(0,2), sqrt(A(1,2)*A(1,2) + A(2,2)*A(2,2)) ); + v[5] = atan2(-A(0,1), A(0,0)); + */ + v[3] = atan2( A(2,1), A(2,2)); + v[4] = atan2(-A(2,0), sqrt(A(2,1)*A(2,1) + A(2,2)*A(2,2)) ); + v[5] = atan2( A(1,0), A(0,0)); + + return v; +} + +dVector CTransformMatrix::GetPositionOrientation2 () const +{ + const dMatrix &A = *this; + dVector v(6); + + v[0] = A(0,3); + v[1] = A(1,3); + v[2] = A(2,3); + // theta in the range (-pi/2, pi/2) + /* + v[3] = atan2( A(1,2), -A(2,2)); + v[4] = atan2( A(0,2), -sqrt(A(1,2)*A(1,2) + A(2,2)*A(2,2)) ); + v[5] = atan2( A(0,1), -A(0,0)); + */ + v[3] = atan2(-A(2,1),-A(2,2)); + v[4] = atan2(-A(2,0),-sqrt(A(2,1)*A(2,1) + A(2,2)*A(2,2)) ); + v[5] = atan2(-A(1,0),-A(0,0)); + + return v; +} + diff --git a/PhantomX-Robot-Arm-Kit/matrix/transformation.h b/PhantomX-Robot-Arm-Kit/matrix/transformation.h new file mode 100644 index 0000000..a3d71e1 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/matrix/transformation.h @@ -0,0 +1,26 @@ +////////////////////////////////////////////////////// +// Vector and Matrix files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#pragma once +#include "matrixAlgebra.h" + +class CTransformMatrix : public dMatrix +{ +public: + CTransformMatrix (); + CTransformMatrix (const matrix &m): dMatrix (m) { } + CTransformMatrix (double x, double y, double z, double phi, double theta, double psi); + + dVector GetRotation (int axis) const; // 변환 행렬에서 축에 대한 회전 성분 벡터를 반환한다. + dVector GetPosition () const; // 변환 행렬의 위치(x, y, z) 벡터를 반환한다. + dVector GetOrientation () const; // 변환 행렬의 방향(phi, theta, psi) 벡터를 반환한다. (theta: -pi/2 ~ pi/2) + dVector GetOrientation2 () const; // 변환 행렬의 방향(phi, theta, psi) 벡터를 반환한다. (theta: pi/2 ~ 3*pi/2) + dVector GetPositionOrientation () const; // 변환 행렬의 위치와 방향 벡터를 반환한다. (theta: -pi/2 ~ pi/2) + dVector GetPositionOrientation2 () const; // 변환 행렬의 위치와 방향 벡터를 반환한다. (theta: pi/2 ~ 3*pi/2) +}; diff --git a/PhantomX-Robot-Arm-Kit/opengl/oglDef.h b/PhantomX-Robot-Arm-Kit/opengl/oglDef.h new file mode 100644 index 0000000..fb1e7e2 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/opengl/oglDef.h @@ -0,0 +1,19 @@ +////////////////////////////////////////////////////// +// OpenGL files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#pragma once + +class CPoint3d { + +public: + CPoint3d() {} + CPoint3d(double _x, double _y, double _z) : x(_x), y(_y), z(_z) { } + double x, y, z; +}; + diff --git a/PhantomX-Robot-Arm-Kit/opengl/oglObjects.cpp b/PhantomX-Robot-Arm-Kit/opengl/oglObjects.cpp new file mode 100644 index 0000000..84ba7fe --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/opengl/oglObjects.cpp @@ -0,0 +1,333 @@ +////////////////////////////////////////////////////// +// OpenGL files for Robotics applications +// +// Copyright (c) 2002-2010. All Rights Reserved. +// Division of Applied Robot Technology, KITECH +// Web: http://www.orobot.net +// Written by KwangWoong Yang +// + +#if defined(_WIN32) || defined(_WIN64) +#include +#elif defined(__APPLE__) +#include +#endif + +#include "matrix/mathDef.h" +#include "oglObjects.h" + +void oglPlane(double size, double stride) +{ + size /= 2.; + int n = (int)(size/stride); + + glDisable(GL_LIGHTING); + + glColor3d(0.5, 0.5, 0.5); + + for (int i=-n ; i<=n; ++i) { + oglLine (i*stride, -size, 0, i*stride, (i == 0) ? 0 : size, 0); + oglLine (-size, i*stride, 0, (i == 0) ? 0 : size, i*stride, 0); + } + // x-axis + glColor3d(1., 0., 0.); + oglLine (0, 0, 0, size, 0, 0); + + // y-axis + glColor3d(0., 1., 0.); + oglLine (0, 0, 0, 0, size, 0); + + // z-axis + glColor3d(0., 0., 1.); + oglLine (0, 0, 0, 0, 0, size); + + glEnable(GL_LIGHTING); +} + +void oglCoordinate(double size) +{ + glDisable(GL_LIGHTING); + + // The Z-axis + glColor3d(0., 0., 1.); + oglLine (0., 0., 0., 0., 0., size); + + // The Y-axis + glColor3d(0., 1., 0.); + oglLine(0., 0., 0., 0., size, 0.); + + // The X-axis + glColor3d(1., 0., 0.); + oglLine(0., 0., 0., size, 0., 0.); + + glEnable(GL_LIGHTING); +} + +void oglLine (double sx, double sy, double sz, double ex, double ey, double ez) +{ + glBegin(GL_LINES); + glVertex3d(sx, sy, sz); + glVertex3d(ex, ey, ez); + glEnd(); +} + +void oglLineStrip(vector &line) +{ + glBegin(GL_LINE_STRIP); + for(unsigned int i=0; i +// + +#pragma once + +#include "oglDef.h" +#include + +using namespace std; + +extern void oglPlane(double size, double stride); +extern void oglCoordinate(double size); + +extern void oglLine (double sx, double sy, double sz, double ex, double ey, double ez); +extern void oglLineStrip (vector &line); +extern void oglSphere (double radius); +extern void oglSphere (double rx, double ry, double rz); +extern void oglCapsule (double height, double radius); +extern void oglCylinder (double height, double radius); +extern void oglCylinder (double height, double baseRadius, double topRadius, double xyRatio); +extern void oglBox (double dx, double dy, double dz); + diff --git a/PhantomX-Robot-Arm-Kit/resource.qrc b/PhantomX-Robot-Arm-Kit/resource.qrc new file mode 100644 index 0000000..72733a4 --- /dev/null +++ b/PhantomX-Robot-Arm-Kit/resource.qrc @@ -0,0 +1,5 @@ + + + image/phantomX-robot-arm-kit.png + + diff --git a/README.md b/README.md index 7021f66..516a0f2 100644 --- a/README.md +++ b/README.md @@ -1 +1,10 @@ -# PhantomX-Robot-Arm-Kit \ No newline at end of file +# PhantomX-Robot-Arm-Kit + +[Trossen Robotics](https://www.trossenrobotics.com/) 회사에서 판매하는 PhantomX Readctor Robot Arm Kit이라는 5축 로봇 팔을 제어하기 위한 소스 + +## 개발환경 +* Qt 5.10.1 (Qt Creator 4.5.1) + +## source 출처 +* [KwangWoong Yang\ Open Robotics Blog ](http://blog.daum.net/pg365/98) +* [Matrix TCL Lite](http://www.techsoftpl.com/matrix/index.php)