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

Last change on this file since 375 was 375, checked in by Sanahuja Guillaume, 12 months 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.