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

Last change on this file since 377 was 377, checked in by Sanahuja Guillaume, 9 months ago

ugv update

File size: 6.2 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  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
70TwoWheelRobot::~TwoWheelRobot() {
71  // les objets irrlicht seront automatiquement detruits par parenté
72}
73
74#ifdef GL
75
76void 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
95void 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
107size_t TwoWheelRobot::dbtSize(void) const {
108  return 3 * sizeof(float) + 2 * sizeof(float); // 3ddl+2motors
109}
110
111void 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
131void 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
158void 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
Note: See TracBrowser for help on using the repository browser.