source: flair-src/trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp@ 375

Last change on this file since 375 was 375, checked in by Sanahuja Guillaume, 2 years ago

update ugv

File size: 5.9 KB
Line 
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
36using namespace irr::video;
37using namespace irr::scene;
38using namespace irr::core;
39#endif
40using namespace flair::core;
41using namespace flair::gui;
42using namespace flair::actuator;
43
44namespace flair {
45namespace simulator {
46
47TwoWheelRobot::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
53
54 Tab *visual_tab = new Tab(GetTabWidget(), "visual");
55 bodyColorR = new SpinBox(visual_tab->NewRow(), "arm color (R):", 0, 255, 1,255);
56 bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
57 bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
58
59 controls = new SimuUgvControls(this, name, modelId,0);
60
61 SetIsReady(true);
62}
63
64TwoWheelRobot::~TwoWheelRobot() {
65 // les objets irrlicht seront automatiquement detruits par parenté
66}
67
68#ifdef GL
69
70void TwoWheelRobot::Draw(void) {
71 // create unite (1m=100cm) robot; scale will be adapted according to settings in gcs
72 // parameter
73 // note that the frame used is irrlicht one:
74 // left handed, North East Up
75 const IGeometryCreator *geo;
76 geo = getGui()->getSceneManager()->getGeometryCreator();
77
78 colored_body = geo->createCubeMesh(vector3df(100,100,100));
79 colored_body->setBoundingBox(aabbox3df(0,0,0,1,1,1));//bug with bounding box? workaround is to reduce it... we use only wheel box
80 MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
81
82 IMesh *wheel = geo->createCylinderMesh(35, 10, 64, SColor(0, 0, 0, 0));
83 MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
84 MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
85
86 ExtraDraw();
87}
88
89void TwoWheelRobot::AnimateModel(void) {
90 if (bodyColorR->ValueChanged() == true || bodyColorG->ValueChanged() == true || bodyColorB->ValueChanged() == true) {
91 getGui()->getSceneManager()->getMeshManipulator()->setVertexColors(colored_body, SColor(0,bodyColorR->Value(), bodyColorG->Value(), bodyColorB->Value()));
92 }
93
94 // adapt robot size
95 if (size->ValueChanged() == true) {
96 setScale(size->Value());
97 }
98
99}
100
101size_t TwoWheelRobot::dbtSize(void) const {
102 return 3 * sizeof(float) + 2 * sizeof(float); // 3ddl+2motors
103}
104
105void TwoWheelRobot::WritedbtBuf(
106 char *dbtbuf) { /*
107 float *buf=(float*)dbtbuf;
108 vector3df vect=getPosition();
109 memcpy(buf,&vect.X,sizeof(float));
110 buf++;
111 memcpy(buf,&vect.Y,sizeof(float));
112 buf++;
113 memcpy(buf,&vect.Z,sizeof(float));
114 buf++;
115 vect=getRotation();
116 memcpy(buf,&vect.X,sizeof(float));
117 buf++;
118 memcpy(buf,&vect.Y,sizeof(float));
119 buf++;
120 memcpy(buf,&vect.Z,sizeof(float));
121 buf++;
122 memcpy(buf,&motors,sizeof(rtsimu_motors));*/
123}
124
125void TwoWheelRobot::ReaddbtBuf(
126 char *dbtbuf) { /*
127 float *buf=(float*)dbtbuf;
128 vector3df vect;
129 memcpy(&vect.X,buf,sizeof(float));
130 buf++;
131 memcpy(&vect.Y,buf,sizeof(float));
132 buf++;
133 memcpy(&vect.Z,buf,sizeof(float));
134 buf++;
135 setPosition(vect);
136 memcpy(&vect.X,buf,sizeof(float));
137 buf++;
138 memcpy(&vect.Y,buf,sizeof(float));
139 buf++;
140 memcpy(&vect.Z,buf,sizeof(float));
141 buf++;
142 ((ISceneNode*)(this))->setRotation(vect);
143 memcpy(&motors,buf,sizeof(rtsimu_motors));
144 AnimateModele();*/
145}
146#endif // GL
147
148// states are computed on fixed frame NED
149// x north
150// y east
151// z down
152void TwoWheelRobot::CalcModel(void) {
153 float speed=0.4, turn=0;
154 Time motorTime;
155
156 controls->GetControls(&speed,&turn,&motorTime);
157
158 // compute quaternion from W
159 // Quaternion derivative: dQ = 0.5*( Q*Qw)
160 state[0].W.x=0;
161 state[0].W.y=0;
162 state[0].W.z=turn;
163 Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
164
165 // Quaternion integration
166 state[0].Quat = state[-1].Quat + dQ * dT();
167 state[0].Quat.Normalize();
168
169 Vector3D<double> dir = Vector3D<double>(speed,0,0);
170 dir.Rotate(state[0].Quat);
171 state[0].Pos = state[-1].Pos + dT() * dir;
172
173
174 /*
175 ** ===================================================================
176 ** z double integrator
177 **
178 ** ===================================================================
179 */
180 state[0].Pos.z = (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
181 state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
182}
183
184} // end namespace simulator
185} // end namespace flair
Note: See TracBrowser for help on using the repository browser.