From 7dc9a5a7fefbb564ed9efeb77365a8aadc754833 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 29 Mar 2017 22:19:39 -0500
Subject: tweak

---
 src/body.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

(limited to 'src/body.cpp')

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  ----- */
 
-- 
cgit v1.1