1 | // %flair:license{
|
---|
2 | // This file is part of the Flair framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %flair:license}
|
---|
5 | // created: 2020/11/20
|
---|
6 | // filename: TwoWheelRobot.cpp
|
---|
7 | //
|
---|
8 | // author: Guillaume Sanahuja
|
---|
9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
10 | //
|
---|
11 | // version: $Id: $
|
---|
12 | //
|
---|
13 | // purpose: classe definissant un TwoWheelRobot
|
---|
14 | //
|
---|
15 | /*********************************************************************/
|
---|
16 |
|
---|
17 | #include "TwoWheelRobot.h"
|
---|
18 | #include <TabWidget.h>
|
---|
19 | #include <Tab.h>
|
---|
20 | #include <DoubleSpinBox.h>
|
---|
21 | #include <SpinBox.h>
|
---|
22 | #include <GroupBox.h>
|
---|
23 | #include <math.h>
|
---|
24 | #include <SimuUgvControls.h>
|
---|
25 | #ifdef GL
|
---|
26 | #include <ISceneManager.h>
|
---|
27 | #include <IMeshManipulator.h>
|
---|
28 | #include "MeshSceneNode.h"
|
---|
29 | #include "Gui.h"
|
---|
30 | #include <Mutex.h>
|
---|
31 | #endif
|
---|
32 |
|
---|
33 | #define G (float)9.81 // gravity ( N/(m/s²) )
|
---|
34 |
|
---|
35 | #ifdef GL
|
---|
36 | using namespace irr::video;
|
---|
37 | using namespace irr::scene;
|
---|
38 | using namespace irr::core;
|
---|
39 | #endif
|
---|
40 | using namespace flair::core;
|
---|
41 | using namespace flair::gui;
|
---|
42 | using namespace flair::actuator;
|
---|
43 |
|
---|
44 | namespace flair {
|
---|
45 | namespace simulator {
|
---|
46 |
|
---|
47 | TwoWheelRobot::TwoWheelRobot(std::string name, uint32_t modelId)
|
---|
48 | : Model(name,modelId) {
|
---|
49 | Tab *setup_tab = new Tab(GetTabWidget(), "model");
|
---|
50 | m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1,1,0.2);
|
---|
51 | size = new DoubleSpinBox(setup_tab->NewRow(), "size (m):", 0, 20, 0.1,1,0.1);
|
---|
52 | t_speed = new DoubleSpinBox(setup_tab->NewRow(), "translational speed (m/s):",
|
---|
53 | 0, 5, 0.1);
|
---|
54 | r_speed = new DoubleSpinBox(setup_tab->NewRow(), "rotational speed (deg/s):",
|
---|
55 | 0, 180, 10);
|
---|
56 |
|
---|
57 |
|
---|
58 | Tab *visual_tab = new Tab(GetTabWidget(), "visual");
|
---|
59 | bodyColorR = new SpinBox(visual_tab->NewRow(), "arm color (R):", 0, 255, 1,255);
|
---|
60 | bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
|
---|
61 | bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
|
---|
62 |
|
---|
63 |
|
---|
64 |
|
---|
65 | controls = new SimuUgvControls(this, name, modelId,0);
|
---|
66 |
|
---|
67 | SetIsReady(true);
|
---|
68 | }
|
---|
69 |
|
---|
70 | TwoWheelRobot::~TwoWheelRobot() {
|
---|
71 | // les objets irrlicht seront automatiquement detruits par parenté
|
---|
72 | }
|
---|
73 |
|
---|
74 | #ifdef GL
|
---|
75 |
|
---|
76 | void TwoWheelRobot::Draw(void) {
|
---|
77 | // create unite (1m=100cm) robot; scale will be adapted according to settings in gcs
|
---|
78 | // parameter
|
---|
79 | // note that the frame used is irrlicht one:
|
---|
80 | // left handed, North East Up
|
---|
81 | const IGeometryCreator *geo;
|
---|
82 | geo = getGui()->getSceneManager()->getGeometryCreator();
|
---|
83 |
|
---|
84 | colored_body = geo->createCubeMesh(vector3df(100,100,100));
|
---|
85 | colored_body->setBoundingBox(aabbox3df(0,0,0,1,1,1));//bug with bounding box? workaround is to reduce it... we use only wheel box
|
---|
86 | MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
|
---|
87 |
|
---|
88 | IMesh *wheel = geo->createCylinderMesh(35, 10, 64, SColor(0, 0, 0, 0));
|
---|
89 | MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
|
---|
90 | MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
|
---|
91 |
|
---|
92 | ExtraDraw();
|
---|
93 | }
|
---|
94 |
|
---|
95 | void TwoWheelRobot::AnimateModel(void) {
|
---|
96 | if (bodyColorR->ValueChanged() == true || bodyColorG->ValueChanged() == true || bodyColorB->ValueChanged() == true) {
|
---|
97 | getGui()->getSceneManager()->getMeshManipulator()->setVertexColors(colored_body, SColor(0,bodyColorR->Value(), bodyColorG->Value(), bodyColorB->Value()));
|
---|
98 | }
|
---|
99 |
|
---|
100 | // adapt robot size
|
---|
101 | if (size->ValueChanged() == true) {
|
---|
102 | setScale(size->Value());
|
---|
103 | }
|
---|
104 |
|
---|
105 | }
|
---|
106 |
|
---|
107 | size_t TwoWheelRobot::dbtSize(void) const {
|
---|
108 | return 3 * sizeof(float) + 2 * sizeof(float); // 3ddl+2motors
|
---|
109 | }
|
---|
110 |
|
---|
111 | void TwoWheelRobot::WritedbtBuf(
|
---|
112 | char *dbtbuf) { /*
|
---|
113 | float *buf=(float*)dbtbuf;
|
---|
114 | vector3df vect=getPosition();
|
---|
115 | memcpy(buf,&vect.X,sizeof(float));
|
---|
116 | buf++;
|
---|
117 | memcpy(buf,&vect.Y,sizeof(float));
|
---|
118 | buf++;
|
---|
119 | memcpy(buf,&vect.Z,sizeof(float));
|
---|
120 | buf++;
|
---|
121 | vect=getRotation();
|
---|
122 | memcpy(buf,&vect.X,sizeof(float));
|
---|
123 | buf++;
|
---|
124 | memcpy(buf,&vect.Y,sizeof(float));
|
---|
125 | buf++;
|
---|
126 | memcpy(buf,&vect.Z,sizeof(float));
|
---|
127 | buf++;
|
---|
128 | memcpy(buf,&motors,sizeof(rtsimu_motors));*/
|
---|
129 | }
|
---|
130 |
|
---|
131 | void TwoWheelRobot::ReaddbtBuf(
|
---|
132 | char *dbtbuf) { /*
|
---|
133 | float *buf=(float*)dbtbuf;
|
---|
134 | vector3df vect;
|
---|
135 | memcpy(&vect.X,buf,sizeof(float));
|
---|
136 | buf++;
|
---|
137 | memcpy(&vect.Y,buf,sizeof(float));
|
---|
138 | buf++;
|
---|
139 | memcpy(&vect.Z,buf,sizeof(float));
|
---|
140 | buf++;
|
---|
141 | setPosition(vect);
|
---|
142 | memcpy(&vect.X,buf,sizeof(float));
|
---|
143 | buf++;
|
---|
144 | memcpy(&vect.Y,buf,sizeof(float));
|
---|
145 | buf++;
|
---|
146 | memcpy(&vect.Z,buf,sizeof(float));
|
---|
147 | buf++;
|
---|
148 | ((ISceneNode*)(this))->setRotation(vect);
|
---|
149 | memcpy(&motors,buf,sizeof(rtsimu_motors));
|
---|
150 | AnimateModele();*/
|
---|
151 | }
|
---|
152 | #endif // GL
|
---|
153 |
|
---|
154 | // states are computed on fixed frame NED
|
---|
155 | // x north
|
---|
156 | // y east
|
---|
157 | // z down
|
---|
158 | void TwoWheelRobot::CalcModel(void) {
|
---|
159 | float speed,turn;
|
---|
160 | Time motorTime;
|
---|
161 |
|
---|
162 | controls->GetControls(&speed,&turn,&motorTime);
|
---|
163 |
|
---|
164 | // compute quaternion from W
|
---|
165 | // Quaternion derivative: dQ = 0.5*( Q*Qw)
|
---|
166 | state[0].W.x=0;
|
---|
167 | state[0].W.y=0;
|
---|
168 | state[0].W.z=turn*r_speed->Value();
|
---|
169 | Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
|
---|
170 |
|
---|
171 | // Quaternion integration
|
---|
172 | state[0].Quat = state[-1].Quat + dQ * dT();
|
---|
173 | state[0].Quat.Normalize();
|
---|
174 |
|
---|
175 | Vector3D<double> dir = Vector3D<double>(speed*t_speed->Value(),0,0);
|
---|
176 | dir.Rotate(state[0].Quat);
|
---|
177 | state[0].Pos = state[-1].Pos + dT() * dir;
|
---|
178 |
|
---|
179 |
|
---|
180 | /*
|
---|
181 | ** ===================================================================
|
---|
182 | ** z double integrator
|
---|
183 | **
|
---|
184 | ** ===================================================================
|
---|
185 | */
|
---|
186 | state[0].Pos.z = (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
|
---|
187 | state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
|
---|
188 | }
|
---|
189 |
|
---|
190 | } // end namespace simulator
|
---|
191 | } // end namespace flair
|
---|