tweak

1 files changed, 4 insertions, 2 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 691a209..041d971 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -29,10 +29,10 @@ Body::dx ( const Matrix<double,9,1> &del )
 {
     X += del;
     // Constrain the height
-    if (X[2]<-1.) {
+    if (X[2]<-1.5) {
         X[2]=-1.;
     } else if (X[2]>-0.3) {
-        X[2]=-0.2;
+        X[2]=-0.3;
     }
     return ;
 }		/* -----  end of method Body::dx  ----- */
@@ -195,6 +195,8 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
 
     X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
+    if (X[2]>-0.3) X[2]=-0.3;
+    if (X[2]<-1.3) X[2]=-1.3;
     return ;
 }		/* -----  end of method Body::motionModel  ----- */