增加最小转弯半径限制，低速时控制转弯半径尽量大，防止前轮摆角震荡。

MPC/loaded_mpc.cpp

@@ -15,6 +15,7 @@ size_t nv = nth + N;
 size_t ndlt = nv + N;
 
 size_t nobs=ndlt+N;
+size_t nwmg=nobs+N;
 
 class FG_eval_half_agv {
  public:
@@ -30,10 +31,7 @@ class FG_eval_half_agv {
 
     typedef CPPAD_TESTVECTOR(CppAD::AD<double>) ADvector;
     void operator()(ADvector& fg, const ADvector& vars) {
-      // Implementing MPC below
-      // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
-      // The cost is stored is the first element of `fg`.
-      // Any additions to the cost should be added to `fg[0]`.
+
       fg[0] = 0;
       double dt=m_condition[0];
       double ref_v=m_condition[1];
@@ -119,11 +117,16 @@ class FG_eval_half_agv {
         fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
         fg[1+ndlt+t]=(vars[ndlt+t]-vars[ndlt+t-1])/dt;
       }
-
+      //与障碍物的距离
       for(int t=0;t<N;++t)
       {
         fg[1+nobs+t]=CppAD::pow(vars[nx+t]-obs_x,2)+CppAD::pow(vars[ny+t]-obs_y,2);
       }
+    //转弯半径
+      /*for(int t=0;t<N;++t)
+      {
+        fg[1+nwmg+t]=CppAD::pow(vars[ndlt+t],2)/(CppAD::pow(vars[nv+t],2)+1e-10);
+      }*/
 
     }
 };
@@ -140,7 +143,18 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   // State vector holds all current values neede for vars below
   Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
   double line_velocity = statu[3];
-  double delta = statu[4];
+  double wmg = statu[4];
+
+  //纠正角速度/线速度，使其满足最小转弯半径
+  double angular=wmg;
+  double radius=25.0*1.3 * (1.0/sqrt(line_velocity*line_velocity+1e-10));
+  if( (line_velocity*line_velocity)/(wmg*wmg+1e-9) < (radius*radius)) {
+    angular = fabs(line_velocity) / radius;
+    if (wmg < 0) angular = -angular;
+  }
+  double max_wmg=fabs(line_velocity) / radius;
+  double min_wmg=-max_wmg;
+
   std::vector<Pose2d> filte_poses;
   if (filte_Path(pose_agv, target, trajectory, filte_poses, 50) == false)
   {
@@ -178,9 +192,9 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   double dt = 0.1;
   double min_velocity = -1.0;
   double max_velocity = 1.0;
-  double max_dlt=2*M_PI/180.0;
-  double max_acc_line_velocity=0.3;
-  double max_acc_dlt=5.*M_PI/180.0;
+  double max_dlt=max_wmg;//5*M_PI/180.0;
+  double max_acc_line_velocity=0.5;
+  double max_acc_dlt=20.*M_PI/180.0;
 
   size_t n_vars = N * 5;
 
@@ -239,9 +253,15 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   double dobs=3.0;
   for(int i=nobs;i<nobs+N;++i)
   {
-    constraints_lowerbound[i] = pow(float(nobs+N-i)/float(N)*dobs,2);
+    constraints_lowerbound[i] = 9;//pow(float(nobs+N-i)/float(N)*dobs,2);
     constraints_upperbound[i] = 1e19;
   }
+  //限制最小转弯半径，
+  /*for(int i=nwmg;i<nwmg+N;++i)
+  {
+    constraints_lowerbound[i] = 0;
+    constraints_upperbound[i] = 1.0/(radius*radius);
+  }*/
 
 
   Eigen::VectorXd statu_velocity(4);
@@ -256,18 +276,18 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     printf(" input -v limited\n");
   }
 
-  if(delta>max_dlt)
+  if(angular>max_dlt)
   {
-    delta = max_dlt;
+    angular = max_dlt;
     printf(" input +dlt limited\n");
   }
-  if(delta<-max_dlt)
+  if(angular<-max_dlt)
   {
-    delta = -max_dlt;
+    angular = -max_dlt;
     printf(" input -dlt limited\n");
   }
 
-  statu_velocity << line_velocity, delta,obs_relative_pose_.x(),obs_relative_pose_.y();
+  statu_velocity << line_velocity, angular,obs_relative_pose_.x(),obs_relative_pose_.y();
 
   Eigen::VectorXd condition(2);
   condition << dt, ref_velocity;
@@ -295,8 +315,8 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
   if (ok == false)
   {
-    printf(" mpc failed statu : %d  input: %.4f  %.4f relative:(%f,%f) \n",solution.status,line_velocity,
-           delta,obs_relative_pose_.x(),obs_relative_pose_.y());
+    printf(" mpc failed statu : %d  input: %.4f  %.5f(%.5f) relative:(%f,%f) \n",solution.status,line_velocity,
+           wmg,angular,obs_relative_pose_.x(),obs_relative_pose_.y());
     return false;
   }
 
@@ -306,14 +326,23 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   out.clear();
 
   double solve_velocity=solution.x[nv];
+  double solve_angular=solution.x[ndlt];
+
+  //纠正角速度/线速度，使其满足最小转弯半径
+  double correct_angular=solve_angular;
+  if( (solve_velocity*solve_velocity)/(solve_angular*solve_angular+1e-9) < (radius*radius)) {
+    correct_angular = fabs(line_velocity) / radius;
+    if (solve_angular < 0) correct_angular = -correct_angular;
+  }
+  ///-----
 
   out.push_back(solve_velocity);
-  double front_delta=solution.x[ndlt];
-  out.push_back(front_delta);
-  printf("input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f time:%.3f\n",
-         line_velocity,
-         delta,solve_velocity,front_delta,ref_velocity,time);
+  out.push_back(correct_angular);
+  printf("input : %.4f  %.5f(%.5f)   output : %.4f  %.5f(%.5f)  ref : %.3f time:%.3f\n",
+         line_velocity,wmg,angular,
+         solve_velocity,solve_angular,correct_angular,ref_velocity,time);
 
+  //计算预测轨迹
   optimize_trajectory.clear();
   for (int i = 0; i < N; ++i)
   {

projects/webots_project/AGV.wbt

@@ -12,12 +12,12 @@ WorldInfo {
   ]
 }
 Viewpoint {
-  orientation 0.5818033438377976 0.5693298869845917 0.5808341836404513 4.18252091947868
-  position 2.8316854176465314 124.06549024084453 47.35952361688281
+  orientation 0.5779733594870706 0.577037331708603 0.5770396117580188 4.19411881474017
+  position 1.6970186753025311 39.104640337250935 20.75749698102983
 }
 DEF BASE Solid {
-  translation 19.999990830376994 0.7911399999999998 0.00016165862726906342
-  rotation 3.813686307174172e-15 1 -7.392696162231656e-14 -0.0015310670374691813
+  translation 19.999990830376994 0.7911399999999998 0.0001616586272690501
+  rotation 6.2920671077854326e-15 1 -5.71413000829659e-14 -0.0015310670374691815
   children [
     DEF BASE Shape {
       appearance Appearance {
@@ -35,12 +35,12 @@ DEF BASE Solid {
   physics Physics {
     density 50
   }
-  linearVelocity 6.859325028145343e-17 4.291439081864306e-16 2.487177595931127e-17
-  angularVelocity 3.641100384136907e-17 4.184616225450991e-18 -9.31478056624474e-17
+  linearVelocity -2.4317884274417482e-18 7.444354048010461e-16 -1.6420350188202083e-17
+  angularVelocity -9.09105400092581e-18 1.0539270092169124e-20 -1.1637627238339089e-17
 }
 DEF Foot Solid {
-  translation 9.999393453313743 0.007628624545976334 -0.08621733915398466
-  rotation -0.5617423039575042 0.5636575435204495 -0.6055871180738632 2.062062846337491
+  translation 9.999393453313743 0.007628624545976376 -0.08621733915398455
+  rotation -0.5617423039575042 0.5636575435204495 -0.6055871180738633 2.062062846337491
   children [
     DEF FootGroup Group {
       children [
@@ -305,8 +305,8 @@ DEF Foot Solid {
   physics Physics {
     density 9800
   }
-  linearVelocity -2.6835060801495474e-16 -1.6305332766423367e-16 2.379287783062859e-16
-  angularVelocity 3.71681486153735e-15 2.1903057875089765e-17 4.014447047551636e-15
+  linearVelocity -2.9224981877109375e-16 -1.6305332766423367e-16 3.651781929038221e-16
+  angularVelocity 5.552068893876114e-15 1.8975918990228804e-17 4.409219118020596e-15
 }
 DEF BODY Solid {
   translation 10 0.1 -1.80464e-16
@@ -547,8 +547,8 @@ DEF BODY Solid {
   boundingObject USE BODY_G
 }
 DEF AGV Robot {
-  translation 1.20997 0.5081 30
-  rotation 0.00028790749930202177 -0.9999997701294638 -0.0006138813332151157 1.5688474942134538
+  translation -3.575526893524931e-05 0.5080978135573649 29.999999946817134
+  rotation 0.00029130142016009605 -0.9999997693242789 -0.0006135917791560724 1.5689585005591844
   children [
     Gyro {
       translation 0 0.4 0
@@ -599,7 +599,7 @@ DEF AGV Robot {
     }
     DEF LFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -1.79684045043229e-06
+        position 1.5567043593433288e-09
         axis 0 1 0
         anchor 1.25 -0.1 0.65
       }
@@ -611,12 +611,12 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2498259680793753 -0.31731160838446126 0.6500300313479261
-        rotation -0.3450388528276186 0.0874773040788691 -0.9345030290536752 0.00017735837807788534
+        translation 1.2498259681333839 -0.31731160838446115 0.6500300316609046
+        rotation -0.3447163559101565 0.09752573202388592 -0.9336269948760199 0.00017752473487954584
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 12714.792964099648
+              position 12714.797020442691
             }
             device [
               RotationalMotor {
@@ -626,8 +626,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 4.3705934254399906e-05 0.0002428590305611499 -0.0011512651861221195
-              rotation -0.999999998609062 -8.449461872153176e-07 -5.273672628566991e-05 2.3741823727955986
+              translation 4.370593425439992e-05 0.0002475269462944203 -0.0011502705978855845
+              rotation -0.999999998606776 -7.385915592760957e-07 -5.2781649475929385e-05 2.370126029744788
               children [
                 DEF WHEEL Shape {
                   appearance Appearance {
@@ -664,6 +664,8 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
+              linearVelocity -2.1332980073888572e-07 -1.9609323212226806e-08 -2.8846914454007297e-08
+              angularVelocity 3.028884669953748e-08 6.345844745903806e-08 2.131848187951259e-06
             }
           }
           DEF FootGroup Group {
@@ -929,11 +931,13 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
+        linearVelocity -1.3803386965892316e-07 -2.4774210118716467e-08 1.7190082289567494e-08
+        angularVelocity 1.827524710516207e-08 6.735916986308837e-08 3.99809655525564e-09
       }
     }
     DEF LBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position 1.909799640451601e-07
+        position 1.1587663067073947e-06
         axis 0 1 0
         anchor 1.25 -0.1 -0.65
       }
@@ -944,8 +948,8 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2502142944867947 -0.3171563536107094 -0.6499857454412845
-        rotation -0.44954716989579785 0.518588054893846 0.7273058306930422 0.00015174308343144375
+        translation 1.25021429450059 -0.31715635361070926 -0.6499857456486758
+        rotation -0.4480582484199508 0.5232275481798828 0.7248977437187357 0.0001522472138164005
         children [
           DEF RightFootGroup Group {
             children [
@@ -1207,7 +1211,7 @@ DEF AGV Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 12089.81243920925
+              position 12089.813230633721
             }
             device [
               RotationalMotor {
@@ -1217,8 +1221,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation -0.0006519639232456022 0.000946668709059292 0.001143714375952278
-              rotation -0.9999999911166082 7.651174463449885e-05 -0.00010914548283850542 1.0002663867689645
+              translation -0.0006519639232456014 0.0009457632491349739 0.001144463234471755
+              rotation -0.9999999911037275 7.661040975261942e-05 -0.00010919427565296956 0.9994749623053355
               children [
                 USE WHEEL
               ]
@@ -1227,6 +1231,8 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
+              linearVelocity -3.0991677508424506e-07 1.5635132885965768e-08 3.9839373741821026e-08
+              angularVelocity -1.0657062909315962e-08 1.208564584294501e-05 3.802765620567589e-07
             }
           }
         ]
@@ -1235,11 +1241,13 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
+        linearVelocity 9.819237450698253e-07 1.7627016642267756e-08 -4.109005810121253e-08
+        angularVelocity -6.153160227031075e-09 1.2245432079301161e-05 8.024265850469229e-09
       }
     }
     DEF RFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -8.891405361579447e-07
+        position -9.25137081992806e-07
         axis 0 1 0
         anchor -1.25 -0.1 0.65
       }
@@ -1250,8 +1258,8 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2499103940445404 -0.31904283227123564 0.6499825577735479
-        rotation -5.22302214154086e-05 0.9999999976721874 -4.3904777433319685e-05 3.1416208160665122
+        translation -1.2499103940439193 -0.31904283227123587 0.6499825577767387
+        rotation -5.22302206336658e-05 0.9999999976721874 -4.390477836327383e-05 3.1416207804562797
         children [
           DEF RightFootGroup Group {
             children [
@@ -1513,7 +1521,7 @@ DEF AGV Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -9401.409793096984
+              position -9401.411542368854
             }
             device [
               RotationalMotor {
@@ -1523,8 +1531,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 1.5974980094312817e-07 -3.936558390461395e-05 -1.2330156882871794e-05
-              rotation 0.9999999999912735 1.058407525554635e-06 4.041384843813483e-06 1.017951379303512
+              translation 1.5974980094312812e-07 -3.938709246182607e-05 -1.226127694455176e-05
+              rotation 0.999999999991246 1.0636093757121596e-06 4.046802108511234e-06 1.0162021074328027
               children [
                 USE WHEEL
               ]
@@ -1533,6 +1541,8 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
+              linearVelocity -1.826082139061124e-07 -1.757957684868586e-08 -1.0291997529528946e-08
+              angularVelocity 1.9299694722190276e-08 -1.1946399272898188e-05 9.186328382096956e-07
             }
           }
         ]
@@ -1541,11 +1551,13 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
+        linearVelocity 9.359643907432107e-07 -1.851756158121073e-08 -1.1542418492388219e-08
+        angularVelocity 1.1094721437079246e-08 -1.2111299958176375e-05 4.3454056614271593e-08
       }
     }
     DEF RBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -2.0564580800644458e-08
+        position -2.2995538465666867e-06
         axis 0 1 0
         anchor -1.25 -0.1 -0.65
       }
@@ -1556,12 +1568,12 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2500226472337264 -0.3190599251737202 -0.6499245258394647
-        rotation -0.00021957244541735292 -0.9999993153212807 0.0011494106799343137 3.1415710519229796
+        translation -1.2500226474057312 -0.3190599251737205 -0.6499245258910776
+        rotation -0.00021957375516195123 -0.9999993153212808 0.0011494104297185448 3.141573330910685
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -9666.451849226816
+              position -9666.451238503458
             }
             device [
               RotationalMotor {
@@ -1571,8 +1583,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 4.1127246297455867e-07 -1.4373436462477738e-05 -0.0002884973014526482
-              rotation -0.999999999999917 9.046138582306803e-08 3.972319567795937e-07 2.9128781971540816
+              translation 4.1127246297455846e-07 -1.419724175205197e-05 -0.0002885060258431959
+              rotation -0.9999999999999171 9.034305187937816e-08 3.972726205487862e-07 2.9122674737249645
               children [
                 USE WHEEL
               ]
@@ -1581,6 +1593,8 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
+              linearVelocity -1.3493402619253419e-07 1.7199408396109328e-08 6.173848819568723e-08
+              angularVelocity -3.810727093997648e-08 -2.7983513165840916e-05 -2.435681363990183e-07
             }
           }
           DEF FootGroup Group {
@@ -1847,6 +1861,8 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
+        linearVelocity 2.5192028586683403e-06 1.6350229142501363e-08 1.639096890748103e-07
+        angularVelocity -2.8304965419442427e-08 -2.847929092887477e-05 5.2388573765506774e-08
       }
     }
     USE BODY_G
@@ -1856,10 +1872,12 @@ DEF AGV Robot {
     density 300
   }
   controller "AGV_controller"
+  linearVelocity -1.866446619235966e-07 -1.6715440234202927e-09 1.5320312093472036e-08
+  angularVelocity 1.0705508316963683e-09 6.968521260411365e-08 3.218818458420577e-08
 }
 DEF AGV_Child Robot {
-  translation 0.012501861789795291 0.5077791024641277 2.6383865754479874e-08
-  rotation 0.027669147222891582 -0.9615319771236447 -0.2732959481233722 0.0038261300703193997
+  translation 0.0009741361622480097 0.5080678890414855 -1.804891521718535e-07
+  rotation 0.1355690878031099 -0.9496560141530762 -0.28244375938428 0.003339030868450933
   children [
     Gyro {
       translation 0 0.4 0
@@ -1910,7 +1928,7 @@ DEF AGV_Child Robot {
     }
     DEF LFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -2.4802110916695665e-06
+        position -2.635759489652905e-07
         axis 0 1 0
         anchor 1.25 -0.1 0.65
       }
@@ -1922,12 +1940,12 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2498259680588435 -0.31731160838446193 0.6500300312289441
-        rotation -0.34515237513599833 0.08365011509426826 -0.9348114762788778 0.00017730000736543874
+        translation 1.2498259681254122 -0.31731160838446204 0.6500300316147093
+        rotation -0.3447661252811035 0.09604439811477256 -0.9337621712456687 0.00017749917457590345
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 12307.700467152443
+              position 12954.516518238224
             }
             device [
               RotationalMotor {
@@ -1937,8 +1955,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation 4.370593425440063e-05 0.0011752270732034837 -5.686267590853934e-05
-              rotation 0.9999999953171763 -5.7891599477254037e-05 7.75513385988885e-05 5.223550950436673
+              translation 4.37059342544006e-05 0.0010835468882463195 -0.00045860459927221095
+              rotation -0.9999999971575736 3.3826044504261155e-05 -6.738435596853268e-05 1.411669909167597
               children [
                 USE WHEEL
               ]
@@ -1947,8 +1965,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -1.2463705531961533e-07 2.702934390871517e-09 1.2584170670532758e-08
-              angularVelocity 4.864752173035862e-07 -2.3303108111081817e-05 -6.949369883311067e-08
+              linearVelocity -1.3695215371655055e-08 3.654812549149753e-10 2.6914071261544313e-09
+              angularVelocity 3.6527405731288046e-08 2.6065813465620475e-06 1.6454003623934172e-09
             }
           }
           DEF FootGroup Group {
@@ -2214,13 +2232,13 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -1.274852691462624e-07 -1.3150612376118644e-09 2.1328814995306245e-06
-        angularVelocity -1.0591834477549399e-07 -2.3659658762424915e-05 -5.0408640937264485e-08
+        linearVelocity 8.351894544083895e-10 5.419041275590905e-10 -2.497701259647864e-07
+        angularVelocity 3.4117620862814205e-09 2.625046481896273e-06 1.4669257053225024e-09
       }
     }
     DEF LBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -1.878497285546203e-06
+        position -9.412601181143394e-08
         axis 0 1 0
         anchor 1.25 -0.1 -0.65
       }
@@ -2231,8 +2249,8 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2502142944572947 -0.3171563536107094 -0.649985744997807
-        rotation -0.4527187831722299 0.5085116796910004 0.7324353725625852 0.00015068044182219758
+        translation 1.2502142944827306 -0.31715635361070926 -0.6499857453801879
+        rotation -0.44998513842548293 0.5172124716479759 0.7280141718180978 0.00015159559375483867
         children [
           DEF RightFootGroup Group {
             children [
@@ -2494,7 +2512,7 @@ DEF AGV_Child Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 11681.691492248954
+              position 12329.380143539183
             }
             device [
               RotationalMotor {
@@ -2504,8 +2522,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation -0.0006519639232456147 0.0005854200882226155 0.001364385406979036
-              rotation 0.9999999832802581 -0.00012524187577806436 0.00013324397157103812 5.5690169233124855
+              translation -0.0006519639232456136 -0.0001707300178342131 0.0014748272710667708
+              rotation -0.9999997813133447 0.0005617039233585294 -0.0003490873323634794 0.19360376425857553
               children [
                 USE WHEEL
               ]
@@ -2514,8 +2532,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity 2.9403652998287883e-08 -7.67486106471827e-09 -4.0037267568602437e-08
-              angularVelocity 3.772312796100735e-07 -1.7661033105430275e-05 5.2192781031318894e-08
+              linearVelocity 4.70633095038395e-09 -2.8760537514484426e-10 8.275743371586595e-10
+              angularVelocity 7.124926833736052e-09 9.039985208922391e-07 -1.4915423949175067e-09
             }
           }
         ]
@@ -2524,13 +2542,13 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity 1.5362640445851933e-09 -4.706795349483696e-09 1.5689997941535845e-06
-        angularVelocity -8.549362698362294e-08 -1.7916506073771674e-05 3.334017753199761e-08
+        linearVelocity 4.617108602279191e-09 -4.399033971768077e-10 -8.66887937170602e-08
+        angularVelocity 7.522927912788431e-10 9.035184738296616e-07 -8.657717675967032e-10
       }
     }
     DEF RFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -2.470709389425638e-06
+        position 1.1232544881643023e-09
         axis 0 1 0
         anchor -1.25 -0.1 0.65
       }
@@ -2541,8 +2559,8 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2499103940169398 -0.3190428322712342 0.64998255791534
-        rotation -5.223018667742951e-05 0.9999999976721873 -4.39048187573072e-05 3.141619233669359
+        translation -1.2499103940600462 -0.3190428322712344 0.6499825576938891
+        rotation -5.2230240931145145e-05 0.9999999976721873 -4.390475421757515e-05 3.1416217050544737
         children [
           DEF RightFootGroup Group {
             children [
@@ -2804,7 +2822,7 @@ DEF AGV_Child Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -9013.053850905386
+              position -9660.735378942676
             }
             device [
               RotationalMotor {
@@ -2814,8 +2832,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation 1.5974980094313124e-07 -2.5706738710861403e-05 3.226213802212933e-05
-              rotation -0.999999999753419 1.6784791061780748e-05 1.4540720922414024e-05 0.1835954742853224
+              translation 1.597498009431311e-07 -6.545648281069974e-06 4.072881600891377e-05
+              rotation 0.9999999999822333 -5.348850222359243e-06 -2.6312047457972025e-06 5.58614843513502
               children [
                 USE WHEEL
               ]
@@ -2824,8 +2842,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -9.479216871629677e-09 6.193487328952021e-09 -1.9738256271295776e-08
-              angularVelocity -4.186455017932854e-07 -2.3212574741223283e-05 -4.046487527771698e-08
+              linearVelocity -7.285231386722021e-09 4.264754505655394e-10 -3.997777135599338e-08
+              angularVelocity 6.982058086618815e-07 -1.9852043442959313e-08 3.963046363098352e-09
             }
           }
         ]
@@ -2834,13 +2852,13 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -7.848574641127519e-09 9.208588502326832e-09 -2.14115896295869e-06
-        angularVelocity 3.7247946052213685e-08 -2.3599254956502662e-05 -2.7725253756786877e-08
+        linearVelocity -1.035103855679066e-08 2.2488782045650652e-10 -4.5078768869644374e-08
+        angularVelocity -1.888099135026936e-09 -1.926650053574915e-08 1.231403085519568e-09
       }
     }
     DEF RBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -2.0782248245816432e-08
+        position 7.647243246198699e-11
         axis 0 1 0
         anchor -1.25 -0.1 -0.65
       }
@@ -2851,12 +2869,12 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2500226472336593 -0.3190599251737188 -0.6499245258394445
-        rotation -0.00021957244490572204 -0.9999993153212808 0.0011494106800320546 3.141571051032729
+        translation -1.250022647232031 -0.31905992517371884 -0.6499245258389559
+        rotation -0.00021957243250686698 -0.9999993153212808 0.0011494106824007533 3.1415710294584174
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -9281.255272816281
+              position -9929.908149471647
             }
             device [
               RotationalMotor {
@@ -2866,8 +2884,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation 4.112724629745624e-07 0.0002758086978941395 8.5830360520856e-05
-              rotation 0.9999999999996374 5.724838541247699e-07 -6.304785558795505e-07 5.292579780444217
+              translation 4.1127246297456237e-07 0.00010920516360451658 -0.000267416381067637
+              rotation -0.9999999999999082 2.214123950404717e-09 4.2830554119214373e-07 2.475395542575079
               children [
                 USE WHEEL
               ]
@@ -2876,8 +2894,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity 2.9065478756883955e-08 -4.037445362981478e-09 -5.588226143914096e-08
-              angularVelocity -4.7563380471949156e-07 3.9043303971723615e-07 4.468780733155552e-08
+              linearVelocity 1.0964265993980248e-08 -5.030613514371658e-10 -4.034754922112983e-08
+              angularVelocity -1.1537703839199734e-06 -1.8947129645325142e-08 -3.451284140273075e-09
             }
           }
           DEF FootGroup Group {
@@ -3144,8 +3162,8 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -5.942246879835356e-08 -7.039117750791243e-09 1.2946692850208225e-07
-        angularVelocity 4.771892723355824e-08 3.935196247470993e-07 3.139072412216777e-08
+        linearVelocity 1.0864718475337499e-08 -7.896209132416773e-10 -4.529027634528878e-08
+        angularVelocity -1.7976852465586598e-09 -2.0549233096079486e-08 1.5522222378240368e-10
       }
     }
     USE BODY_G
@@ -3156,8 +3174,8 @@ DEF AGV_Child Robot {
     density 300
   }
   controller "agv_child"
-  linearVelocity -1.18411132873794e-08 -6.094527601784615e-10 -1.4096933303990628e-08
-  angularVelocity -8.956879261476707e-09 1.9204272072681243e-07 -1.533348904728203e-09
+  linearVelocity -1.4282542700859824e-09 -9.174576146645589e-11 -2.3890280083318287e-08
+  angularVelocity -7.146617851630384e-10 -1.820740413366924e-08 9.345840100610298e-11
 }
 TexturedBackground {
 }