Changeset 375 in flair-src for trunk/lib/FlairSimulator

Timestamp:

Dec 9, 2020, 2:32:49 PM (4 years ago)

Author:

Sanahuja Guillaume

Message:

update ugv

Location:

trunk/lib/FlairSimulator/src

Files:

• Model_impl.cpp (modified) (2 diffs)
• SimuUgvControls.cpp (added)
• SimuUgvControls.h (added)
• TwoWheelRobot.cpp (modified) (6 diffs)
• TwoWheelRobot.h (modified) (2 diffs)

trunk/lib/FlairSimulator/src/Model_impl.cpp

@@ -229 +229 @@
   // TODO: setEllipsoidRadius should be called in Model::setScale
   // but we need to call recalculateBoundingBox
+  
   anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
-
+ 
   if (anim->collisionOccurred() == true) {
     vector3df pos;
@@ -238 +239 @@
     nodePosition = getPosition();
     pos_rel = pos - nodePosition;
-    // printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
-    // printf("object %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
-    // printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
+     //printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
+     //printf("object %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
+     //printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
 
     collision_mutex->GetMutex();

trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp

@@ -22 +22 @@
 #include <GroupBox.h>
 #include <math.h>
+#include <SimuUgvControls.h>
 #ifdef GL
 #include <ISceneManager.h>
@@ -56 +57 @@
   bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
   
+  controls = new SimuUgvControls(this, name, modelId,0);
+  
   SetIsReady(true);
 }
 
 TwoWheelRobot::~TwoWheelRobot() {
-  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
-  // pales) par parenté
+  // les objets irrlicht seront automatiquement detruits par parenté
 }
 
@@ -75 +77 @@
 
   colored_body = geo->createCubeMesh(vector3df(100,100,100));
+  colored_body->setBoundingBox(aabbox3df(0,0,0,1,1,1));//bug with bounding box? workaround is to reduce it... we use only wheel box
   MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
   
@@ -80 +83 @@
   MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
   MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
-  ExtraDraw();
+  
+ ExtraDraw();
 }
 
@@ -147 +151 @@
 // z down
 void TwoWheelRobot::CalcModel(void) {
-  state[0].Pos.x=state[-1].Pos.x;
-  state[0].Pos.y=state[-1].Pos.y;
-  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
-  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
+  float speed=0.4, turn=0;
+  Time motorTime;
+
+  controls->GetControls(&speed,&turn,&motorTime);
+          
+  // compute quaternion from W
+  // Quaternion derivative: dQ = 0.5*(  Q*Qw)
+  state[0].W.x=0;
+  state[0].W.y=0;
+  state[0].W.z=turn;
+  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
+
+  // Quaternion integration
+  state[0].Quat = state[-1].Quat + dQ * dT();
+  state[0].Quat.Normalize();
+
+  Vector3D<double> dir = Vector3D<double>(speed,0,0);
+  dir.Rotate(state[0].Quat);
+  state[0].Pos = state[-1].Pos + dT() * dir;
+  
  
-  //Printf("%f %f %f\n",state[0].Pos.x,state[-1].Pos.x,state[-2].Pos.x);
   /*
   ** ===================================================================
@@ -159 +178 @@
   ** ===================================================================
   */
-  state[0].Pos.z =
-      (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
-  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
-//Printf("%f %f %f\n",state[0].Pos.z,state[-1].Pos.z,state[-2].Pos.z);
-#ifndef GL
-  if (state[0].Pos.z < 0)
-    state[0].Pos.z = 0;
-#endif
+  state[0].Pos.z = (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
   state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
 }

trunk/lib/FlairSimulator/src/TwoWheelRobot.h

@@ -29 +29 @@
 }
 namespace actuator {
-class SimuBldc;
+class SimuUgvControls;
 }
 }
@@ -65 +65 @@
   gui::SpinBox *bodyColorR,*bodyColorG,*bodyColorB;
   gui::DoubleSpinBox *size,*m;
+  actuator::SimuUgvControls *controls;