12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

One line definition of gravity force.

release/4.3a0
Alex Hagiopol 2016-03-12 20:49:00 -05:00
parent 93bf31f852
commit c769ed8452
1 changed files with 1 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -90,8 +90,7 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Vector Fu = computeFu(gamma2, u2);
Vector fGravity_g1 = zero(6);
Vector subVector = g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81));
fGravity_g1.segment(3,subVector.size()) = subVector;
fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame
Vector Fb = Fu+fGravity_g1;
Vector dV = newtonEuler(V1_g1, Fb, Inertia);