├── Ch5.pdf
├── scurve.o
├── 03478.pdf
├── libscurve.so
├── libscurve.so.1
├── libscurve.so.1.0
├── libscurve.so.1.0.0
├── gui_project
├── arrow-down.png
├── libicons.qrc
├── main.cpp
├── motion.pro
├── mainwindow.h
├── libocct
│ ├── opencascade.h
│ ├── draw_primitives.h
│ ├── opencascade.cpp
│ └── draw_primitives.cpp
├── pallete.xml
├── color_pallete.xml
├── mainwindow.ui
└── mainwindow.cpp
├── scurve.pro
├── scurve.h
├── README.md
├── scurve.cpp
└── Makefile
/Ch5.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/Ch5.pdf
--------------------------------------------------------------------------------
/scurve.o:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/scurve.o
--------------------------------------------------------------------------------
/03478.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/03478.pdf
--------------------------------------------------------------------------------
/libscurve.so:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/libscurve.so
--------------------------------------------------------------------------------
/libscurve.so.1:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/libscurve.so.1
--------------------------------------------------------------------------------
/libscurve.so.1.0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/libscurve.so.1.0
--------------------------------------------------------------------------------
/libscurve.so.1.0.0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/libscurve.so.1.0.0
--------------------------------------------------------------------------------
/gui_project/arrow-down.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/grotius-cnc/s-curve-motion-planning/HEAD/gui_project/arrow-down.png
--------------------------------------------------------------------------------
/gui_project/libicons.qrc:
--------------------------------------------------------------------------------
1 |
2 |
3 | arrow-down.png
4 |
5 |
6 |
--------------------------------------------------------------------------------
/gui_project/main.cpp:
--------------------------------------------------------------------------------
1 | #include "mainwindow.h"
2 |
3 | #include
4 |
5 | int main(int argc, char *argv[])
6 | {
7 | QApplication a(argc, argv);
8 | MainWindow w;
9 | w.show();
10 | return a.exec();
11 | }
12 |
--------------------------------------------------------------------------------
/scurve.pro:
--------------------------------------------------------------------------------
1 | QT -= gui core
2 |
3 | TEMPLATE = lib
4 | DEFINES += SCURVE_LIBRARY
5 |
6 | CONFIG += c++11
7 |
8 | # You can make your code fail to compile if it uses deprecated APIs.
9 | # In order to do so, uncomment the following line.
10 | #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
11 |
12 | SOURCES += \
13 | scurve.cpp
14 |
15 | HEADERS += \
16 | scurve.h
17 |
18 | # Default rules for deployment.
19 | unix {
20 | target.path = /usr/lib
21 | }
22 | !isEmpty(target.path): INSTALLS += target
23 |
--------------------------------------------------------------------------------
/scurve.h:
--------------------------------------------------------------------------------
1 | #ifndef SCURVE_H
2 | #define SCURVE_H
3 |
4 | #include "iostream"
5 | #include "chrono"
6 |
7 | //! This shared library is written by Skynet Cyberdyne.
8 | //! No green pass is required to use this software, just use your mind.
9 |
10 | class Scurve
11 | {
12 | public:
13 | Scurve();
14 |
15 | struct RESULT{
16 | //! Displacement result.
17 | double sr=0;
18 | //! Velocity result.
19 | double vr=0;
20 | //! Acceleration result.
21 | double ar=0;
22 | //! Curve time.
23 | double ct=0;
24 | //! Curve displacment.
25 | double cs=0;
26 | //! Error.
27 | bool error=0;
28 | };
29 |
30 | //! Inputs:
31 | //! at_time=request displacment at a certain time stamp.
32 | //! vs=velocity max.
33 | //! cs=curve displacement
34 | RESULT scurve_lineair(double at_time, double vs, double cs);
35 |
36 | //! Inputs:
37 | //! sct=scurve type [0=acc, 1=dcc]
38 | //! vo=velocity start
39 | //! vs=velocity max
40 | //! ve=velocity end
41 | //! am=acceleration max
42 | //! acs=acceleration start
43 | //! ace=acceleration end
44 | //! Results:
45 | //! at_time=request curve at [t]
46 | RESULT scurve_acc_dcc(int sct, double vo, double ve, double am, double acs, double ace, double at_time);
47 |
48 | //! Inputs:
49 | //! vs=velocity max.
50 | //! am=acceleration max.
51 | //! vo=velocity begin.
52 | //! acs=acceleration begin.
53 | //! ltot=pathlenght.
54 | //! ve=velocity end.
55 | //! ace=acceleration end.
56 | //! at_time=curve request at time [t]
57 | RESULT motion(double vs, double am, double vo, double acs, double ltot, double ve, double ace, double at_time);
58 |
59 | void example();
60 | };
61 |
62 |
63 | #endif // SCURVE_H
64 |
65 |
66 |
67 |
68 |
69 |
70 |
71 |
72 |
73 |
74 |
75 |
76 |
77 |
78 |
79 |
80 |
81 |
82 |
83 |
84 |
--------------------------------------------------------------------------------
/gui_project/motion.pro:
--------------------------------------------------------------------------------
1 | QT += core gui opengl
2 |
3 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
4 |
5 | CONFIG += c++17
6 |
7 | # You can make your code fail to compile if it uses deprecated APIs.
8 | # In order to do so, uncomment the following line.
9 | #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
10 |
11 | SOURCES += \
12 | libocct/draw_primitives.cpp \
13 | libocct/opencascade.cpp \
14 | main.cpp \
15 | mainwindow.cpp
16 |
17 | HEADERS += \
18 | libocct/draw_primitives.h \
19 | libocct/opencascade.h \
20 | mainwindow.h
21 |
22 | FORMS += \
23 | mainwindow.ui
24 |
25 | INCLUDEPATH += /usr/include/ \
26 | /usr/local/include/opencascade/ \
27 | /usr/include/eigen3/ \
28 | /usr/local/lib/ \
29 |
30 | LIBS += -L/usr/local/lib/ \
31 |
32 | # project
33 | INCLUDEPATH+= libspline/ \
34 | libocct/ \
35 |
36 | #Opencascade
37 | LIBS+= -L/usr/local/lib/ \
38 |
39 | INCLUDEPATH += /usr/local/include/kdl/ \
40 | /usr/local/include/opencascade/ \
41 | /usr/include/eigen3/ \
42 | /usr/include/ \
43 | /usr/local/lib/ \
44 |
45 | LIBS += -lorocos-kdl
46 |
47 | # Opencascade
48 | LIBS += -lTKPrim
49 | LIBS += -lTKernel -lTKMath -lTKTopAlgo -lTKService
50 | LIBS += -lTKG2d -lTKG3d -lTKV3d -lTKOpenGl
51 | LIBS += -lTKBRep -lTKXSBase -lTKGeomBase
52 | LIBS += -lTKMeshVS -lTKXSDRAW
53 | LIBS += -lTKLCAF -lTKXCAF -lTKCAF
54 | LIBS += -lTKCDF -lTKBin -lTKBinL -lTKBinXCAF -lTKXml -lTKXmlL -lTKXmlXCAF
55 | # -- IGES support
56 | LIBS += -lTKIGES
57 | # -- STEP support
58 | LIBS += -lTKSTEP -lTKXDESTEP -lTKXDEIGES
59 | # -- STL support
60 | LIBS += -lTKSTL
61 | # -- OBJ/glTF support
62 |
63 | LIBS += -lTKRWMesh
64 |
65 | #src/base/io_occ_base_mesh.cpp \
66 | #src/base/io_occ_gltf.cpp \
67 | #src/base/io_occ_obj.cpp
68 |
69 | # -- VRML support
70 | LIBS += -lTKVRML
71 |
72 | # this copies the configuration files etc to the build direcory. So user has only to edit the source directory.
73 | copydata.commands = $(COPY_DIR) $$PWD/* $$OUT_PWD
74 | first.depends = $(first) copydata
75 | export(first.depends)
76 | export(copydata.commands)
77 | QMAKE_EXTRA_TARGETS += first copydata
78 |
79 | # Default rules for deployment.
80 | qnx: target.path = /tmp/$${TARGET}/bin
81 | else: unix:!android: target.path = /opt/$${TARGET}/bin
82 | !isEmpty(target.path): INSTALLS += target
83 |
84 | RESOURCES += \
85 | libicons.qrc
86 |
--------------------------------------------------------------------------------
/gui_project/mainwindow.h:
--------------------------------------------------------------------------------
1 | #ifndef MAINWINDOW_H
2 | #define MAINWINDOW_H
3 |
4 | #include
5 |
6 | #ifdef Success
7 | #undef Success
8 | #endif
9 |
10 | // libocc
11 | #include
12 | using namespace occ;
13 |
14 | #include
15 |
16 | QT_BEGIN_NAMESPACE
17 | namespace Ui { class MainWindow; }
18 | QT_END_NAMESPACE
19 |
20 | class MainWindow : public QMainWindow
21 | {
22 | Q_OBJECT
23 |
24 | public:
25 | MainWindow(QWidget *parent = nullptr);
26 | ~MainWindow();
27 |
28 | struct RESULT{
29 | //! Displacement result.
30 | double sr=0;
31 | //! Velocity result.
32 | double vr=0;
33 | //! Acceleration result.
34 | double ar=0;
35 | //! Curve time.
36 | double ct=0;
37 | //! Curve displacment.
38 | double cs=0;
39 | //! Error.
40 | bool error=0;
41 | };
42 |
43 | RESULT scurve_lineair(double at_time, double vs, double cs);
44 | RESULT scurve_acc_dcc(int sct, double vo, double ve, double am, double acs, double ace, double at_time);
45 | RESULT motion(double vs, double am, double vo, double acs, double ltot, double ve, double ace, double at_time);
46 |
47 | struct CONTROL{
48 | //! Maximum velocity
49 | double vs=0;
50 | //! Velocity overide 0-100%
51 | double vso=0;
52 | //! Maximum acceleration
53 | double am=0;
54 | //! Interval time.
55 | double i_time=0;
56 | //! Traject time.
57 | double tr_time=0;
58 | //! Traject displacment.
59 | double tr_s=0;
60 | };
61 |
62 | struct PATH{
63 | //! Maximum velocity.
64 | double vs=0;
65 | //! Velocity begin.
66 | double vo=0;
67 | //! Acceleration start.
68 | double acs=0;
69 | //! Path lenght.
70 | double ltot=0;
71 | //! Velocity end.
72 | double ve=0;
73 | //! Acceleration end.
74 | double ace=0;
75 | //! Distance to go.
76 | double dtg=0;
77 | };
78 |
79 | void simulate();
80 | void runner(CONTROL c, std::vector pathvec);
81 |
82 | private slots:
83 |
84 | void on_pushButton_show_acc_dcc_curve_pressed();
85 |
86 | void on_doubleSpinBox_velocity_start_valueChanged(double arg1);
87 |
88 | void on_doubleSpinBox_velocity_end_valueChanged(double arg1);
89 |
90 | void on_doubleSpinBox_acceleration_max_valueChanged(double arg1);
91 |
92 | void on_doubleSpinBox_acceleration_begin_valueChanged(double arg1);
93 |
94 | void on_comboBox_scruve_type_currentIndexChanged(int index);
95 |
96 | void on_doubleSpinBox_lin_velocity_valueChanged(double arg1);
97 |
98 | void on_doubleSpinBox_lin_displacment_valueChanged(double arg1);
99 |
100 | void on_pushButton_show_lin_curve_pressed();
101 |
102 | void on_pushButton_clear_lin_curve_pressed();
103 |
104 | void on_pushButton_create_motion_block_pressed();
105 |
106 | void on_pushButton_clear_dcc_curve_pressed();
107 |
108 | void on_doubleSpinBox_mb_velocity_start_valueChanged(double arg1);
109 |
110 | void on_doubleSpinBox_mb_velocity_end_valueChanged(double arg1);
111 |
112 | void on_doubleSpinBox_mb_velocity_max_valueChanged(double arg1);
113 |
114 | void on_doubleSpinBox_mb_acc_max_valueChanged(double arg1);
115 |
116 | void on_doubleSpinBox_mb_acc_begin_valueChanged(double arg1);
117 |
118 | void on_doubleSpinBox_mb_acc_end_valueChanged(double arg1);
119 |
120 | void on_pushButton_mb_clear_pressed();
121 |
122 | void on_doubleSpinBox_mb_pathlenght_valueChanged(double arg1);
123 |
124 | void on_doubleSpinBox_acceleration_end_valueChanged(double arg1);
125 |
126 | void on_pushButton_simulate_path_pressed();
127 |
128 | private:
129 | Ui::MainWindow *ui;
130 | Opencascade *OpencascadeWidget;
131 |
132 | };
133 | #endif // MAINWINDOW_H
134 |
135 |
--------------------------------------------------------------------------------
/gui_project/libocct/opencascade.h:
--------------------------------------------------------------------------------
1 | #ifndef OPENCASCADE_H
2 | #define OPENCASCADE_H
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 |
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 |
17 | #include
18 | #ifdef _WIN32
19 | #include
20 | #else
21 | #undef None
22 | #include
23 | #endif
24 |
25 | #include
26 | #include
27 | #include
28 | #include
29 | #include
30 | #include
31 |
32 | //show xyz axis
33 | #include
34 | #include
35 |
36 | #include
37 |
38 | namespace occ {
39 | class Opencascade: public QGLWidget
40 | {
41 | Q_OBJECT
42 | public:
43 | explicit Opencascade(QWidget *parent = nullptr);
44 |
45 | struct io {
46 | std::string halcommand={""};
47 | };
48 |
49 | struct bucket {
50 | std::string primitivetype; // line,wire,arc,circle,spline.
51 | std::vector pointvec;
52 | std::vector eulervec;
53 | Handle(AIS_Shape) Ais_shape;
54 | std::string info;
55 |
56 | // Hal io
57 | std::vector iovec;
58 |
59 | double vo=0,ve=0,velmax=0,accmax=0;
60 | };
61 | std::vector bucketvec;
62 |
63 | std::vector previewbucketvec;
64 |
65 | int ready=0;
66 |
67 | void Init_robot();
68 | void setup_tcp_origin();
69 |
70 | void show_shape(Handle(AIS_Shape) shape);
71 | void Redraw();
72 |
73 | // Preview line
74 | void draw_preview_cone(std::string type, gp_Trsf trsf);
75 | void empty_preview_bucket();
76 |
77 | // View
78 | void Zoom_all();
79 | void Set_orthographic();
80 | void Set_perspective();
81 | void set_view_front();
82 | void set_view_back();
83 | void set_view_left();
84 | void set_view_right();
85 | void set_view_top();
86 | void set_view_bottom();
87 |
88 | // Selection
89 | void get_selections();
90 | void delete_selections();
91 |
92 | void Erase_all();
93 | void Remove_all();
94 |
95 | public:
96 | //Handle(AIS_InteractiveContext) m_context;
97 | private:
98 | void m_initialize_context();
99 | Handle(AIS_InteractiveContext) m_context;
100 | Handle(V3d_Viewer) m_viewer;
101 | Handle(V3d_View) m_view;
102 | Handle(Graphic3d_GraphicDriver) m_graphic_driver;
103 | Handle(AIS_InteractiveObject) m_aisViewCube;
104 |
105 | // Xyz axis sign.
106 | Handle(Geom_Axis2Placement) axis;
107 | Handle(AIS_Trihedron) aisTrihedron;
108 | std::vector aisTrihedrons;
109 |
110 | protected:
111 | void paintEvent(QPaintEvent *);
112 | void resizeEvent(QResizeEvent *);
113 | void mousePressEvent(QMouseEvent *event);
114 | void mouseReleaseEvent(QMouseEvent *event);
115 | void mouseMoveEvent(QMouseEvent *event);
116 | void wheelEvent(QWheelEvent *event);
117 |
118 | protected:
119 | enum CurrentAction3d
120 | {
121 | CurAction3d_Nothing,
122 | CurAction3d_DynamicPanning,
123 | CurAction3d_DynamicZooming,
124 | CurAction3d_DynamicRotation
125 | };
126 |
127 | private:
128 | Standard_Integer m_x_max;
129 | Standard_Integer m_y_max;
130 | CurrentAction3d m_current_mode;
131 | //gp_Trsf current_tcp;
132 |
133 | Handle(AIS_Shape) aisBody_tcp_xaxis, aisBody_tcp_yaxis, aisBody_tcp_zaxis;
134 |
135 | // Create the euler lines
136 | double toollenght=105;
137 | double linelenght=25;
138 | double coneheight=25;
139 | double conetopdiameter=1;
140 | double conebottomdiameter=5;
141 | double textheight=25;
142 |
143 |
144 | TopoDS_Edge edge_linepreview;
145 | Handle(AIS_Shape) aisBody_linepreview;
146 |
147 | signals:
148 |
149 | public slots:
150 | };
151 | }
152 |
153 | #endif // OPENCASCADE_H
154 |
155 |
156 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # S-curve-motion-planning.
2 |
3 | [](https://github.com/grotius-cnc/s-curve-motion-planning "Go to GitHub repo")
4 | [](https://github.com/grotius-cnc/s-curve-motion-planning)
5 | [](https://github.com/grotius-cnc/s-curve-motion-planning)
6 |
7 | [](https://github.com/grotius-cnc/s-curve-motion-planning/releases/)
8 | [](#license)
9 |
10 | The purpose of this material is to impart an understanding of polynomial transitions for a trajectory generator. The ideal constant jerk S-curve
11 | (jerk is the derivate of acceleration and a measure of impact) can be represented by a second-order polynomial in velocity (3rd in position).
12 | Its shape is governed by the motion conditions at the start and end of the transition.
13 |
14 | An S-curve with an intermediate constant acceleration (lineair portion) is often used to reduce the time to make large speed changes. The jerk can be
15 | used to determine how much of the rise or fall period can be made under constant acceleration.
16 |
17 | ## Implementation:
18 |
19 | - Velocity up s-curve [acc period].
20 | - Velocity down s-curve [dcc period].
21 | - Acceleration begin value.
22 | - Acceleration end value.
23 | - Velocity begin.
24 | - Velocity end.
25 | - Max acceleration.
26 | - Lineair stage.
27 | - Scientific papers included.
28 |
29 | ## Functions:
30 |
31 | //! Inputs:
32 | //! at_time=request displacment at a certain time stamp.
33 | //! vs=velocity max.
34 | //! cs=curve displacement
35 | RESULT scurve_lineair(double at_time, double vs, double cs);
36 |
37 | //! Inputs:
38 | //! sct=scurve type [0=acc, 1=dcc]
39 | //! vo=velocity start
40 | //! vs=velocity max
41 | //! ve=velocity end
42 | //! am=acceleration max
43 | //! acs=acceleration start
44 | //! ace=acceleration end
45 | //! Results:
46 | //! at_time=request curve at [t]
47 | RESULT scurve_acc_dcc(int sct, double vo, double ve, double am, double acs, double ace, double at_time);
48 |
49 | //! Inputs:
50 | //! vs=velocity max.
51 | //! am=acceleration max.
52 | //! vo=velocity begin.
53 | //! acs=acceleration begin.
54 | //! ltot=pathlenght.
55 | //! ve=velocity end.
56 | //! ace=acceleration end.
57 | //! at_time=curve request at time [t]
58 | RESULT motion(double vs, double am, double vo, double acs, double ltot, double ve, double ace, double at_time);
59 |
60 | ## Result:
61 |
62 | struct RESULT{
63 | //! Displacement result.
64 | double sr=0;
65 | //! Velocity result.
66 | double vr=0;
67 | //! Acceleration result.
68 | double ar=0;
69 | //! Curve time.
70 | double ct=0;
71 | //! Curve displacment.
72 | double cs=0;
73 | //! Error.
74 | bool error=0;
75 | };
76 |
77 | ## Build in Logic:
78 |
79 | - If maximum velocity can not be reached for a motion, curves are sampled to fit.
80 | - Debug information when input values are out of scope.
81 | - In the /gui_project is a simple path planner example.
82 |
83 | ## Performance:
84 |
85 | Time taken by function nanoseconds: ~1955 milliseconds: ~0.002
86 |
87 | ## Graphics
88 |
89 | ~/gui_project/motion/
90 |
91 | 
92 |
93 | To use the opencascade graphics along with the gui project, follow these instructions :
94 | https://github.com/grotius-cnc/oce/releases/tag/1.0.1
95 |
96 | ## Summary
97 | Constant jerk S-curves are used to transition robot moves. The S-curve is used to eliminate discontinuities in acceleration that are detrimental
98 | to robot performance.
99 |
100 | ## License
101 |
102 | Released under [MIT](/LICENSE) by [@grotius-cnc](https://github.com/grotius-cnc).
103 |
--------------------------------------------------------------------------------
/gui_project/pallete.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 | 202
8 | 202
9 | 202
10 |
11 |
12 |
13 |
14 |
15 |
16 | 33
17 | 38
18 | 45
19 |
20 |
21 |
22 |
23 |
24 |
25 | 255
26 | 255
27 | 255
28 |
29 |
30 |
31 |
32 |
33 |
34 | 163
35 | 163
36 | 163
37 |
38 |
39 |
40 |
41 |
42 |
43 | 13
44 | 17
45 | 23
46 |
47 |
48 |
49 |
50 |
51 |
52 | 13
53 | 17
54 | 23
55 |
56 |
57 |
58 |
59 |
60 |
61 | 56
62 | 56
63 | 56
64 |
65 |
66 |
67 |
68 |
69 |
70 |
71 |
72 | 202
73 | 202
74 | 202
75 |
76 |
77 |
78 |
79 |
80 |
81 | 33
82 | 38
83 | 45
84 |
85 |
86 |
87 |
88 |
89 |
90 | 255
91 | 255
92 | 255
93 |
94 |
95 |
96 |
97 |
98 |
99 | 163
100 | 163
101 | 163
102 |
103 |
104 |
105 |
106 |
107 |
108 | 13
109 | 17
110 | 23
111 |
112 |
113 |
114 |
115 |
116 |
117 | 13
118 | 17
119 | 23
120 |
121 |
122 |
123 |
124 |
125 |
126 | 56
127 | 56
128 | 56
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 | 190
138 | 190
139 | 190
140 |
141 |
142 |
143 |
144 |
145 |
146 | 33
147 | 38
148 | 45
149 |
150 |
151 |
152 |
153 |
154 |
155 | 190
156 | 190
157 | 190
158 |
159 |
160 |
161 |
162 |
163 |
164 | 190
165 | 190
166 | 190
167 |
168 |
169 |
170 |
171 |
172 |
173 | 13
174 | 17
175 | 23
176 |
177 |
178 |
179 |
180 |
181 |
182 | 13
183 | 17
184 | 23
185 |
186 |
187 |
188 |
189 |
190 |
191 | 56
192 | 56
193 | 56
194 |
195 |
196 |
197 |
198 |
199 |
--------------------------------------------------------------------------------
/gui_project/color_pallete.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 | 255
8 | 255
9 | 255
10 |
11 |
12 |
13 |
14 |
15 |
16 | 65
17 | 65
18 | 65
19 |
20 |
21 |
22 |
23 |
24 |
25 | 255
26 | 255
27 | 255
28 |
29 |
30 |
31 |
32 |
33 |
34 | 255
35 | 255
36 | 255
37 |
38 |
39 |
40 |
41 |
42 |
43 | 65
44 | 65
45 | 65
46 |
47 |
48 |
49 |
50 |
51 |
52 | 65
53 | 65
54 | 65
55 |
56 |
57 |
58 |
59 |
60 |
61 | 255
62 | 255
63 | 255
64 |
65 |
66 |
67 |
68 |
69 |
70 | 255
71 | 255
72 | 255
73 |
74 |
75 |
76 |
77 |
78 |
79 |
80 |
81 | 255
82 | 255
83 | 255
84 |
85 |
86 |
87 |
88 |
89 |
90 | 65
91 | 65
92 | 65
93 |
94 |
95 |
96 |
97 |
98 |
99 | 255
100 | 255
101 | 255
102 |
103 |
104 |
105 |
106 |
107 |
108 | 255
109 | 255
110 | 255
111 |
112 |
113 |
114 |
115 |
116 |
117 | 65
118 | 65
119 | 65
120 |
121 |
122 |
123 |
124 |
125 |
126 | 65
127 | 65
128 | 65
129 |
130 |
131 |
132 |
133 |
134 |
135 | 255
136 | 255
137 | 255
138 |
139 |
140 |
141 |
142 |
143 |
144 | 255
145 | 255
146 | 255
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 | 190
156 | 190
157 | 190
158 |
159 |
160 |
161 |
162 |
163 |
164 | 65
165 | 65
166 | 65
167 |
168 |
169 |
170 |
171 |
172 |
173 | 190
174 | 190
175 | 190
176 |
177 |
178 |
179 |
180 |
181 |
182 | 190
183 | 190
184 | 190
185 |
186 |
187 |
188 |
189 |
190 |
191 | 65
192 | 65
193 | 65
194 |
195 |
196 |
197 |
198 |
199 |
200 | 65
201 | 65
202 | 65
203 |
204 |
205 |
206 |
207 |
208 |
209 | 255
210 | 255
211 | 255
212 |
213 |
214 |
215 |
216 |
217 |
218 | 0
219 | 0
220 | 0
221 |
222 |
223 |
224 |
225 |
226 |
--------------------------------------------------------------------------------
/gui_project/libocct/draw_primitives.h:
--------------------------------------------------------------------------------
1 | #ifndef DRAW_PRIMITIVES_H
2 | #define DRAW_PRIMITIVES_H
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 | #include
26 | #include
27 | #include
28 | #include
29 | #include
30 | #include
31 | #include
32 | #include
33 | #include
34 | #include
35 | #include
36 | #include
37 | #include
38 | #include
39 | #include
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 | #include
47 | #include
48 | #include
49 | #include
50 | #include
51 | #include
52 | #include
53 | #include
54 | #include
55 | #include
56 | #include
57 | #include
58 | #include
59 | #include
60 | #include
61 | #include
62 | #include
63 | #include
64 | #include
65 | #include
66 | #include
67 | #include
68 | #include
69 | #include
70 | #include
71 | #include
72 | #include "Geom_Axis2Placement.hxx"
73 | #include
74 | #include
75 | #include
76 | #include
77 | #include
78 | #include
79 | #include
80 | #include
81 | #include
82 | #include
83 | #include
84 | #include
85 | #include
86 | #include
87 | #include
88 | #include
89 | #include
90 |
91 | #include "XCAFPrs_DocumentExplorer.hxx"
92 | #include
93 | #include
94 | #include
95 |
96 | #include
97 | #include
98 | #include
99 |
100 | #include "gp_Elips.hxx"
101 |
102 | #include
103 | #include
104 | #include
105 | #include
106 | #include
107 |
108 | #include
109 | #include
110 |
111 | #include
112 | #include
113 | #include
114 | #include
115 |
116 | #ifdef Success
117 | #undef Success
118 | #endif
119 |
120 | //using namespace Eigen;
121 |
122 | class draw_primitives
123 | {
124 | public:
125 | draw_primitives();
126 |
127 | // Draw 2d primitives:
128 | Handle(AIS_Shape) draw_2d_circle(gp_Pnt center,double radius);
129 | Handle(AIS_Shape) draw_cp_2d_arc(gp_Pnt center, gp_Pnt point1, gp_Pnt point2);
130 | Handle(AIS_Shape) draw_2d_acad_arc(gp_Pnt center, double radius, const Standard_Real alpha1, const Standard_Real alpha2);
131 | Handle(AIS_Shape) draw_2d_ellipse(gp_Pnt center, gp_Pnt secpoint, double alpha_start, double alpha_end, double ratio);
132 | Handle(AIS_Shape) draw_2d_text(std::string str, double textheight, gp_Pnt point, double rot_deg);
133 |
134 | // Draw 3d primitives:
135 | Handle(AIS_Shape) draw_3d_point(gp_Pnt point);
136 | Handle(AIS_Shape) draw_3d_line(gp_Pnt point1, gp_Pnt point2);
137 | Handle(AIS_Shape) draw_3d_line_wire(std::vector points);
138 | Handle(AIS_Shape) draw_3p_3d_arc(gp_Pnt point1, gp_Pnt point2, gp_Pnt point3);
139 | Handle(AIS_Shape) draw_3p_3d_circle(gp_Pnt point1,gp_Pnt point2,gp_Pnt point3);
140 | Handle(AIS_Shape) draw_3d_spline(std::vector pointvec, int divisions); // Divisions is spline resolution (number of line fragments / segmentation value).
141 |
142 | // Draw 3d solids:
143 | Handle(AIS_Shape) draw_3d_cone(gp_Pnt centerpoint, double bottomdiameter, double topdiameter, double height);
144 | Handle(AIS_Shape) draw_3d_tcp_cone(gp_Pnt centerpoint, double bottomdiameter, double topdiameter, double height);
145 | Handle(AIS_Shape) draw_3d_cilinder(double radius, double height);
146 | Handle(AIS_Shape) draw_3d_sphere(double radius, gp_Pnt center);
147 | Handle(AIS_Shape) draw_3d_box(double dx, double dy, double dz);
148 |
149 | // Draw 3d tools:
150 | std::vector draw_3d_arc_lenght(gp_Pnt point1,gp_Pnt point2,gp_Pnt point3);
151 | double get_3d_arc_lenght(gp_Pnt point1,gp_Pnt point2,gp_Pnt point3, int divisions);
152 | gp_Pnt get_3d_arc_point_given_arclenght_fromstartpoint(gp_Pnt point1,gp_Pnt point2,gp_Pnt point3, double arclenght_from_startpoint);
153 | Handle(AIS_Shape) rotate_3d(Handle(AIS_Shape) shape, gp_Pnt center, double euler_z, double euler_y, double euler_x);
154 | Handle(AIS_Shape) rotate_translate_3d_quaternion(Handle(AIS_Shape) shape, gp_Pnt translation, double euler_z, double euler_y, double euler_x);
155 | Handle(AIS_Shape) translate_3d(Handle(AIS_Shape) shape, gp_Pnt current, gp_Pnt target);
156 | Handle(AIS_Shape) colorize(Handle(AIS_Shape) shape, Quantity_Color color, double transparancy);
157 | gp_Pnt midpoint(gp_Pnt point1, gp_Pnt point2);
158 |
159 | // Draw 3d sets:
160 | Handle(AIS_Shape) draw_3d_origin(gp_Pnt origin, double linelenght);
161 | Handle(AIS_Shape) draw_3d_point_origin_cone(gp_Pnt point, gp_Pnt euler);
162 | Handle(AIS_Shape) draw_3d_point_origin_cone_text(gp_Pnt point, gp_Pnt euler, std::string text, int textheight, int textrotation);
163 | Handle(AIS_Shape) draw_3d_line_origin_cone_text(gp_Pnt point1, gp_Pnt point2, gp_Pnt euler1, gp_Pnt euler2, std::string text, int textheight);
164 | Handle(AIS_Shape) draw_3d_wire_origin_cone_text(std::vector points, std::vector euler, std::string text, int textheight);
165 | Handle(AIS_Shape) draw_3d_arc_origin_cone_text(std::vector points, std::vector euler, std::string text, int textheight);
166 | Handle(AIS_Shape) draw_3d_circle_origin_cone_text(std::vector points, std::vector euler, std::string text, int textheight);
167 | Handle(AIS_Shape) draw_3d_spline_origin_cone_text(std::vector points, std::vector euler, int divisions, std::string text, int textheight);
168 |
169 |
170 | };
171 |
172 | #endif // DRAW_PRIMITIVES_H
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
--------------------------------------------------------------------------------
/scurve.cpp:
--------------------------------------------------------------------------------
1 | #include "scurve.h"
2 |
3 | Scurve::Scurve()
4 | {
5 | }
6 |
7 | void Scurve::example(){
8 | std::cout.precision(3);
9 |
10 | //! Velocity max.
11 | double vs=10;
12 | //! Acceleration max.
13 | double am=2;
14 |
15 | //! Pathlenght.
16 | double ltot=100;
17 |
18 | //! Velocity begin.
19 | double vo=0;
20 | //! Acceleration begin.
21 | double acs=0;
22 |
23 | //! Velocity end.
24 | double ve=0;
25 | //! Acceleration end.
26 | double ace=0;
27 |
28 | double t=0;
29 |
30 | //! A motion.
31 | bool ok=1;
32 | while(ok){
33 |
34 | RESULT r=motion(vs, am, vo, acs, ltot, ve, ace, t);
35 | t+=0.1;
36 | //! Exit when time > result.curvetime.
37 | if(t>=r.ct){
38 | ok=0;
39 | }
40 |
41 | std::cout<vs){
55 | ve=vs;
56 | }
57 |
58 | //! Priority, starting at acc<0, controlled stop, goto to vs.
59 | if(acs<=0 && vs=0, controlled stop, goto to vs.
67 | if(acs>0 && vs=a.ct){
86 | t-=a.ct;
87 | r=scurve_acc_dcc(1,vo+gain, vs, am, 0/*acs*/, ace ,t);
88 | r.sr+=a.cs;
89 | }
90 |
91 | r.ct=ct;
92 | return r;
93 | }
94 |
95 | //! Normal curve, starting with acc>0
96 | if(acs>=0 && vs>=vo && vs>=ve){
97 | //! Get curve time.
98 | t=0;
99 | RESULT a,b,c;
100 | double ct=0;
101 | a=scurve_acc_dcc(0,vo, vs, am, acs, 0/*ace*/ ,t);
102 | c=scurve_acc_dcc(1,vs, ve, am, 0/*acs*/, ace ,t);
103 |
104 | double lb=ltot-(a.cs+c.cs);
105 | b=scurve_lineair(0,vs,lb);
106 | ct=a.ct+b.ct+c.ct;
107 |
108 | if(lb<0){
109 | std::cout<<"lin stage<0!"<ltot){
112 | vs-=0.1;
113 | a=scurve_acc_dcc(0,vo, vs, am, acs, 0/*ace*/ ,t);
114 | c=scurve_acc_dcc(1,vs, ve, am, 0/*acs*/, ace ,t);
115 | }
116 | lb=ltot-(a.cs+c.cs);
117 |
118 | b=scurve_lineair(t,vs,lb);
119 | ct=a.ct+b.ct+c.ct;
120 | }
121 |
122 | t=at_time;
123 | if(t<=a.ct){
124 | r=scurve_acc_dcc(0,vo, vs, am, acs, 0/*ace*/ ,t);
125 | }
126 | if(t>a.ct && t<(a.ct+b.ct)){
127 | t-=a.ct;
128 | r=scurve_lineair(t,vs,lb);
129 | r.sr+=a.cs;
130 | }
131 | if(t>=a.ct+b.ct && t<=a.ct+b.ct+c.ct){
132 | t-=a.ct;
133 | t-=b.ct;
134 | r=scurve_acc_dcc(1,vs, ve, am, 0/*acs*/, ace ,t);
135 | r.sr+=a.cs;
136 | r.sr+=b.cs;
137 |
138 |
139 | }
140 | r.ct=ct;
141 | return r;
142 | }
143 |
144 | //! Normal curve, starting with acc<0
145 | if(acs<0 && vs>=vo && vs>=ve){
146 | //! Get curve time.
147 | t=0;
148 | RESULT a,b,c,d;
149 | double ct=0;
150 | a=scurve_acc_dcc(1,vo, vo-1, am, acs, 0/*ace*/ ,t);
151 | b=scurve_acc_dcc(0,vo-1, vs, am, 0/*acs*/, 0/*ace*/ ,t);
152 | d=scurve_acc_dcc(1,vs, ve, am, 0/*acs*/, ace ,t);
153 |
154 | double lc=ltot-(a.cs+b.cs+d.cs);
155 | c=scurve_lineair(0,vs,lc);
156 | ct=a.ct+b.ct+c.ct+d.ct;
157 |
158 | if(lc<0){
159 | std::cout<<"lin stage<0!"<ltot){
162 | vs-=0.1;
163 | a=scurve_acc_dcc(1,vo, vo-1, am, acs, 0/*ace*/ ,t);
164 | b=scurve_acc_dcc(0,vo-1, vs, am, 0/*acs*/, 0/*ace*/ ,t);
165 | d=scurve_acc_dcc(1,vs, ve, am, 0/*acs*/, ace ,t);
166 | }
167 | lc=ltot-(a.cs+b.cs+d.cs);
168 |
169 | c=scurve_lineair(t,vs,lc);
170 | ct=a.ct+b.ct+c.ct+d.ct;
171 | }
172 |
173 | t=at_time;
174 | if(t=a.ct && t<(a.ct+b.ct)){
178 | t-=a.ct;
179 | r=scurve_acc_dcc(0,vo-1, vs, am, 0/*acs*/, 0/*ace*/ ,t);
180 | r.sr+=a.cs;
181 | }
182 | if(t>=a.ct+b.ct && t=a.ct+b.ct+c.ct && t<=a.ct+b.ct+c.ct+d.ct){
190 | t-=a.ct;
191 | t-=b.ct;
192 | t-=c.ct;
193 | r=scurve_acc_dcc(1,vs, ve, am, 0/*acs*/, ace ,t);
194 | r.sr+=a.cs;
195 | r.sr+=b.cs;
196 | r.sr+=c.cs;
197 | }
198 | r.ct=ct;
199 | return r;
200 | }
201 |
202 | return r;
203 | }
204 |
205 | //! Inputs:
206 | //! sct=scurve type [0=acc, 1=dcc]
207 | //! vo=velocity start
208 | //! vs=velocity max
209 | //! ve=velocity end
210 | //! am=acceleration max
211 | //! acs=acceleration start
212 | //! ace=acceleration end
213 | //! at_time=request curve state at time stamp.
214 | Scurve::RESULT Scurve::scurve_acc_dcc(int sct, double vo, double ve, double am, double acs, double ace, double at_time){
215 |
216 | RESULT r;
217 |
218 | //! ***** = Addons for scurve mirror effect.
219 | if(sct==1){
220 | //! ***** mirror acceleration start, end.
221 | double temp_acs=std::abs(acs);
222 | acs=std::abs(ace);
223 | ace=temp_acs;
224 |
225 | //! ***** Mirror ve & vo.
226 | double temp_ve=ve;
227 | ve=vo;
228 | vo=temp_ve;
229 | }
230 |
231 | //! Minimal maximum acceleration.
232 | if(am<0.1){
233 | am=0.1;
234 | std::cout<<"am set to:"<(2*am)){
237 | acs=2*am;
238 | std::cout<<"acs limited to:"<2*am){
241 | ace=2*am;
242 | std::cout<<"ace limited to:"< velocity start.
245 | if(vo>=ve){
246 | std::cout<<"impossible situation for acc curve. vo>ve"<0){
272 | ts=acs/jm; //! Time[t] on concave period. a=jm*t;
273 | } else {
274 | ts=0;
275 | }
276 | //! Curve has acceleration end value. Find end time[t].
277 | if(ace>0){
278 | te=(as-ace)/jm; //! Time[t] on concvex period. a=jm*t;
279 | } else {
280 | te=t1/2;
281 | }
282 |
283 | //! Concave period.
284 | t=ts; //! at start of curve.
285 | double sa=vo*t+jm*(t*t*t)/6; //! Netto displacment concave front.
286 | t=t1/2; //! at end of curve.
287 | vece=vo+jm*(t*t)/2; //! velocity end.
288 | double sb=vo*t+jm*(t*t*t)/6; //! Bruto displacement concave front.
289 | sce=sb-sa; //! Netto displacment convace back.
290 |
291 |
292 | //! Convex period.
293 | th=t1/2;
294 | vh=vo+jm*(th*th)/2;
295 | t=te;
296 | vecx=vh + as*t - jm*(t*t)/2; //! Convex velocity end.
297 | scx=vh*t + as*(t*t)/2 - jm*(t*t*t)/6; //! Netto displacement convex front.
298 | t=t1/2;
299 | double sc=vh*t + as*(t*t)/2 - jm*(t*t*t)/6; //! Bruto displacement.
300 | double sd=sc-scx; //! Netto displacement convex back.
301 |
302 | double stn=0; //! Curve total netto displacment, excl. acc & dcc start value displacments.
303 | stn+=sce;
304 | stn+=scx;
305 | r.cs=stn; //! Curve displacment
306 |
307 | double concave=(t1/2)-ts;
308 | double convex=te;
309 | double ct=concave+convex; //! Netto curve time [ct]
310 |
311 | if(sct==1){
312 | //! ***** Mirror time.
313 | at_time=ct-at_time;
314 | if(at_time<0){at_time=0;}
315 | }
316 |
317 | if(at_time<=ct){
318 | if(at_time<=concave){
319 | //! ts is relative startpos if start acc is used.
320 | t=at_time+ts;
321 | r.sr=vo*t+jm*(t*t*t)/6; //! Displacement.
322 | r.sr-=vo*ts+jm*(ts*ts*ts)/6; //! Substract front acc.
323 | r.vr=vo+jm*(t*t)/2; //! Velocity.
324 | r.ar=jm*t; //! Acceleration.
325 |
326 | if(sct==1){ //! **** Scurve type : dcc.
327 | r.ar=-std::abs(r.ar); //! **** Dcc curve.
328 | r.sr=stn-r.sr; //! **** Dcc curve, mirror displacment.
329 | }
330 |
331 | }
332 | if(at_time>concave){
333 | t=at_time-concave;
334 |
335 | th=t1/2;
336 | vh=vo+jm*(th*th)/2; //! Velocity at start of convex period. Velocity at first inflection point.
337 |
338 | r.sr=vh*t + as*(t*t)/2 - jm*(t*t*t)/6;
339 | r.sr+=sce; //! Add displacement concave period.
340 | r.vr=vh + as*t - jm*(t*t)/2;
341 | r.ar=as-jm*t;
342 |
343 | if(sct==1){ //! **** Scurve type : dcc.
344 | r.ar=-std::abs(r.ar); //! ***** Dcc curve.
345 | r.sr=stn-r.sr; //! **** Dcc curve, mirror displacment.
346 | }
347 | }
348 | }
349 |
350 | //! This does nothing now, but i leave it here just for info.
351 | bool debug=0;
352 | if(debug){
353 | std::cout<<"Check displacements"<
2 |
3 | MainWindow
4 |
5 |
6 |
7 | 0
8 | 0
9 | 1215
10 | 740
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 | 202
20 | 202
21 | 202
22 |
23 |
24 |
25 |
26 |
27 |
28 | 33
29 | 38
30 | 45
31 |
32 |
33 |
34 |
35 |
36 |
37 | 255
38 | 255
39 | 255
40 |
41 |
42 |
43 |
44 |
45 |
46 | 163
47 | 163
48 | 163
49 |
50 |
51 |
52 |
53 |
54 |
55 | 13
56 | 17
57 | 23
58 |
59 |
60 |
61 |
62 |
63 |
64 | 13
65 | 17
66 | 23
67 |
68 |
69 |
70 |
71 |
72 |
73 | 56
74 | 56
75 | 56
76 |
77 |
78 |
79 |
80 |
81 |
82 |
83 |
84 | 202
85 | 202
86 | 202
87 |
88 |
89 |
90 |
91 |
92 |
93 | 33
94 | 38
95 | 45
96 |
97 |
98 |
99 |
100 |
101 |
102 | 255
103 | 255
104 | 255
105 |
106 |
107 |
108 |
109 |
110 |
111 | 163
112 | 163
113 | 163
114 |
115 |
116 |
117 |
118 |
119 |
120 | 13
121 | 17
122 | 23
123 |
124 |
125 |
126 |
127 |
128 |
129 | 13
130 | 17
131 | 23
132 |
133 |
134 |
135 |
136 |
137 |
138 | 56
139 | 56
140 | 56
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 | 190
150 | 190
151 | 190
152 |
153 |
154 |
155 |
156 |
157 |
158 | 33
159 | 38
160 | 45
161 |
162 |
163 |
164 |
165 |
166 |
167 | 190
168 | 190
169 | 190
170 |
171 |
172 |
173 |
174 |
175 |
176 | 190
177 | 190
178 | 190
179 |
180 |
181 |
182 |
183 |
184 |
185 | 13
186 | 17
187 | 23
188 |
189 |
190 |
191 |
192 |
193 |
194 | 13
195 | 17
196 | 23
197 |
198 |
199 |
200 |
201 |
202 |
203 | 56
204 | 56
205 | 56
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 | Skynet cyberdyne s-curve builder
214 |
215 |
216 |
217 | :/arrow-down.png:/arrow-down.png
218 |
219 |
220 |
221 |
222 |
223 |
224 | -
225 |
226 |
-
227 |
228 |
229 | -10000.000000000000000
230 |
231 |
232 | 10000.000000000000000
233 |
234 |
235 | 0.500000000000000
236 |
237 |
238 | 0.000000000000000
239 |
240 |
241 |
242 | -
243 |
244 |
245 | Velocity max (Vs)
246 |
247 |
248 |
249 | -
250 |
251 |
252 | Velocity start (Vo)
253 |
254 |
255 |
256 | -
257 |
258 |
259 | Result = Time [t]
260 |
261 |
262 |
263 | -
264 |
265 |
266 | Acceleration End
267 |
268 |
269 |
270 | -
271 |
272 |
273 | Curve acc,dcc
274 |
275 |
276 |
277 | -
278 |
279 |
280 | -10000.000000000000000
281 |
282 |
283 | 10000.000000000000000
284 |
285 |
286 | 3.000000000000000
287 |
288 |
289 |
290 | -
291 |
292 |
293 | -10000.000000000000000
294 |
295 |
296 | 10000.000000000000000
297 |
298 |
299 | 0.100000000000000
300 |
301 |
302 | 1.000000000000000
303 |
304 |
305 |
306 | -
307 |
308 |
309 | Velocity start (Vo)
310 |
311 |
312 |
313 | -
314 |
315 |
316 | Acceleration Begin
317 |
318 |
319 |
320 | -
321 |
322 |
323 | -10000.000000000000000
324 |
325 |
326 | 10000.000000000000000
327 |
328 |
329 | 0.000000000000000
330 |
331 |
332 |
333 | -
334 |
335 |
336 | -10000.000000000000000
337 |
338 |
339 | 10000.000000000000000
340 |
341 |
342 | 10.000000000000000
343 |
344 |
345 |
346 | -
347 |
348 |
349 | Velocity (Ve)
350 |
351 |
352 |
353 | -
354 |
355 |
356 | Acceleration Max (AccMax)
357 |
358 |
359 |
360 | -
361 |
362 |
363 | Clear
364 |
365 |
366 |
367 | -
368 |
369 |
370 | Acceleration Max (AccMax)
371 |
372 |
373 |
374 | -
375 |
376 |
377 | Velocity end (Ve)
378 |
379 |
380 |
381 | -
382 |
383 |
384 | -10000.000000000000000
385 |
386 |
387 | 10000.000000000000000
388 |
389 |
390 | 0.500000000000000
391 |
392 |
393 | 5.000000000000000
394 |
395 |
396 |
397 | -
398 |
399 |
400 | -10000.000000000000000
401 |
402 |
403 | 10000.000000000000000
404 |
405 |
406 | 0.100000000000000
407 |
408 |
409 |
410 | -
411 |
412 |
413 | Show dcc curve
414 |
415 |
416 |
417 | -
418 |
419 |
420 | -10000.000000000000000
421 |
422 |
423 | 10000.000000000000000
424 |
425 |
426 | 5.000000000000000
427 |
428 |
429 |
430 | -
431 |
432 |
433 | -10000.000000000000000
434 |
435 |
436 | 10000.000000000000000
437 |
438 |
439 | 0.100000000000000
440 |
441 |
442 |
443 | -
444 |
445 |
446 | Clear
447 |
448 |
449 |
450 | -
451 |
452 |
453 | Show linair curve
454 |
455 |
456 |
457 | -
458 |
459 |
460 | Motion block
461 |
462 |
463 |
464 | -
465 |
466 |
467 | Curve lineair
468 |
469 |
470 |
471 | -
472 |
473 |
474 | -10000.000000000000000
475 |
476 |
477 | 10000.000000000000000
478 |
479 |
480 | 10.000000000000000
481 |
482 |
483 |
484 | -
485 |
486 |
487 | Displacement (S)
488 |
489 |
490 |
491 | -
492 |
493 |
494 | Velocity end (Ve)
495 |
496 |
497 |
498 | -
499 |
500 |
-
501 |
502 | Scurve type::acceleration. Velocity up.
503 |
504 |
505 | -
506 |
507 | Scurve type::deceleration. Velocity down.
508 |
509 |
510 |
511 |
512 | -
513 |
514 |
515 | Acceleration Begin
516 |
517 |
518 |
519 | -
520 |
521 |
522 | Acceleration End
523 |
524 |
525 |
526 | -
527 |
528 |
529 | -10000.000000000000000
530 |
531 |
532 | 10000.000000000000000
533 |
534 |
535 | 0.000000000000000
536 |
537 |
538 |
539 | -
540 |
541 |
542 | -10000.000000000000000
543 |
544 |
545 | 100000.000000000000000
546 |
547 |
548 | 2.000000000000000
549 |
550 |
551 |
552 | -
553 |
554 |
555 | -10000.000000000000000
556 |
557 |
558 | 10000.000000000000000
559 |
560 |
561 |
562 | -
563 |
564 |
565 | Pahtlenght
566 |
567 |
568 |
569 | -
570 |
571 |
572 | -100000.000000000000000
573 |
574 |
575 | 100000.000000000000000
576 |
577 |
578 | 1.000000000000000
579 |
580 |
581 | 100.000000000000000
582 |
583 |
584 |
585 | -
586 |
587 |
588 | Create motion block
589 |
590 |
591 |
592 | -
593 |
594 |
595 | Clear
596 |
597 |
598 |
599 | -
600 |
601 |
602 | Simulate path example
603 |
604 |
605 |
606 |
607 |
608 | -
609 |
610 |
611 |
612 | 0
613 | 0
614 |
615 |
616 |
617 |
-
618 |
619 |
620 |
621 |
622 |
623 |
624 |
625 |
626 |
627 |
628 |
629 |
630 |
631 |
--------------------------------------------------------------------------------
/gui_project/libocct/opencascade.cpp:
--------------------------------------------------------------------------------
1 | #include "opencascade.h"
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 | #include
26 | #include
27 | #include
28 | #include
29 | #include
30 | #include
31 | #include
32 | #include
33 | #include
34 | #include
35 | #include
36 | #include
37 | #include
38 | #include
39 | #include
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 | #include
47 | #include
48 | #include
49 | #include
50 | #include
51 | #include
52 | #include
53 | #include
54 | #include
55 | #include
56 | #include
57 | #include
58 | #include
59 | #include
60 | #include
61 | #include
62 | #include
63 | #include
64 | #include
65 | #include "Geom_Axis2Placement.hxx"
66 | #include
67 | #include
68 | #include
69 | #include
70 | #include
71 | #include
72 | #include
73 | #include
74 | #include
75 | #include
76 | #include
77 | #include
78 | #include
79 | #include
80 | #include
81 | #include
82 | #include
83 |
84 | #include "XCAFPrs_DocumentExplorer.hxx"
85 | #include
86 | #include
87 | #include
88 |
89 | #include
90 | #include
91 | #include
92 | #include
93 |
94 | #include "gp_Elips.hxx"
95 | #include
96 | #include
97 |
98 | //! Make conversion's easy:
99 | #define toRadians M_PI/180.0
100 | #define toDegrees (180.0/M_PI)
101 |
102 | using namespace occ;
103 |
104 | Opencascade::Opencascade(QWidget *parent) : QGLWidget(parent)
105 | {
106 | setBackgroundRole( QPalette::NoRole );
107 | setMouseTracking( true );
108 | }
109 |
110 | void Opencascade::setup_tcp_origin(){
111 | double toollenght=105;
112 | TopoDS_Edge edge = BRepBuilderAPI_MakeEdge({525+toollenght,0,890},{525+toollenght+100,0,890});
113 | aisBody_tcp_xaxis = new AIS_Shape(edge);
114 | //aisBody_tcp_xaxis->SetLocalTransformation(level0x1x2x3x4x5x6);
115 | m_context->SetColor(aisBody_tcp_xaxis,Quantity_NOC_RED,Standard_False);
116 | m_context->SetMaterial(aisBody_tcp_xaxis,Graphic3d_NOM_PLASTIC,Standard_False);
117 | m_context->Display(aisBody_tcp_xaxis,Standard_False);
118 |
119 | edge= BRepBuilderAPI_MakeEdge({525+toollenght,0,890},{525+toollenght,0+100,890});
120 | aisBody_tcp_yaxis = new AIS_Shape(edge);
121 | //aisBody_tcp_yaxis->SetLocalTransformation(level0x1x2x3x4x5x6);
122 | m_context->SetColor(aisBody_tcp_yaxis,Quantity_NOC_GREEN,Standard_False);
123 | m_context->SetMaterial(aisBody_tcp_yaxis,Graphic3d_NOM_PLASTIC,Standard_False);
124 | m_context->Display(aisBody_tcp_yaxis,Standard_False);
125 |
126 | edge= BRepBuilderAPI_MakeEdge({525+toollenght,0,890},{525+toollenght,0,890+100});
127 | aisBody_tcp_zaxis = new AIS_Shape(edge);
128 | //aisBody_tcp_zaxis->SetLocalTransformation(level0x1x2x3x4x5x6);
129 | m_context->SetColor(aisBody_tcp_zaxis,Quantity_NOC_BLUE,Standard_False);
130 | m_context->SetMaterial(aisBody_tcp_zaxis,Graphic3d_NOM_PLASTIC,Standard_False);
131 | m_context->Display(aisBody_tcp_zaxis,Standard_False);
132 | }
133 |
134 | void Opencascade::show_shape(Handle(AIS_Shape) shape){
135 | m_context->Display(shape,Standard_False);
136 | }
137 |
138 | void Opencascade::draw_preview_cone(std::string type, gp_Trsf trsf){
139 |
140 | gp_Pnt point={525+toollenght-coneheight,0,890};
141 | gp_Dir xDirection(1,0,0); // Direction is auto normalized by occ.
142 | gp_Dir normalDirection(0,0,1);
143 | gp_Ax2 aplace(point,normalDirection,xDirection);
144 |
145 | TopoDS_Shape t_topo_cone = BRepPrimAPI_MakeCone(aplace,conebottomdiameter,conetopdiameter,coneheight).Shape();
146 |
147 | // Draw toolpos cone at startpoint
148 | previewbucketvec.push_back(new AIS_Shape(t_topo_cone));
149 |
150 | gp_Trsf MyTrsf_Local_Rot;
151 | MyTrsf_Local_Rot.SetRotation(gp_Ax1(gp_Pnt(
152 | 525+toollenght-coneheight,
153 | 0,
154 | 890), gp_Dir(
155 | 0, //rotation flag x
156 | 1, //rotation flag y
157 | 0)), 90 * toRadians);
158 |
159 | previewbucketvec.back()->SetLocalTransformation(trsf*MyTrsf_Local_Rot);
160 |
161 | if(type=="startpoint"){
162 | m_context->SetColor(previewbucketvec.back(),Quantity_NOC_GREEN,Standard_False);
163 | }
164 | if(type=="io"){
165 | m_context->SetColor(previewbucketvec.back(),Quantity_NOC_RED,Standard_False);
166 | }
167 | if(type=="waypoint"){
168 | m_context->SetColor(previewbucketvec.back(),Quantity_NOC_BLUE,Standard_False);
169 | }
170 | if(type=="endpoint"){
171 | m_context->SetColor(previewbucketvec.back(),Quantity_NOC_BLACK,Standard_False);
172 | }
173 |
174 | m_context->SetTransparency(previewbucketvec.back(),0.5,false);
175 | m_context->SetMaterial(previewbucketvec.back(),Graphic3d_NOM_PLASTIC,Standard_False);
176 | m_context->Display(previewbucketvec.back(),Standard_False);
177 | }
178 |
179 | void Opencascade::empty_preview_bucket(){
180 | for(unsigned int i=0; i0){ // At least one is selected.
183 | m_context->SetSelected(previewbucketvec.at(i),false);
184 | m_context->EraseSelected(false);
185 | }
186 | }
187 | previewbucketvec.clear();
188 | }
189 |
190 | void Opencascade::get_selections(){ // Updated by jointpos function from mainwindow.
191 |
192 | for(m_context->InitSelected(); m_context->MoreSelected(); m_context->NextSelected()){
193 |
194 | const TopoDS_Shape& aSelShape = m_context->SelectedShape();
195 | std::cout<<"selected shape type:"<SelectedShape()==bucketvec.at(i).Ais_shape->Shape()){
200 | std::cout<<"match found at Ais_bucket i:"< 7
248 | // TopAbs_SHAPE
249 | // */
250 |
251 | // std::cout<<"inside shape enum 7"<InitSelected(); m_context->MoreSelected(); m_context->NextSelected()){
277 |
278 | // Find the match of selected item in the Ais_databucket.
279 | // In this case one item can be removed each time.
280 | for(unsigned int i=0; iSelectedShape()==bucketvec.at(i).Ais_shape->Shape()){
283 | //std::cout<<"match found, remove this item from Ais_bucket"<EraseSelected(false);
292 |
293 | // For check print content.
294 |
295 | for(unsigned int i=0; iShape(), TopAbs_EDGE); explorer.More(); explorer.Next()){
299 |
300 | const TopoDS_Edge& edge = TopoDS::Edge(explorer.Current());
301 |
302 | TopoDS_Vertex v1,v2;
303 | TopExp::Vertices(edge,v1,v2);
304 | gp_Pnt p1= BRep_Tool::Pnt(v1);
305 | gp_Pnt p2= BRep_Tool::Pnt(v2);
306 |
307 | std::cout<<"content left edge p1 x: "<