diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 6 |
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 ----- */ |