Skip to content

Commit

Permalink
Create fresh Inertial object and enable all tests
Browse files Browse the repository at this point in the history 
In c++ the test object is recreated in different scopes,
but it was being reused in the python test and accumulating
error.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@scpeters
scpeters committed Nov 8, 2021
1 parent 563bb30 commit 60ceeea
Showing 1 changed file with 18 additions and 18 deletions.
36 changes: 18 additions & 18 deletions src/python/Inertial_TEST.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,7 @@ def SetRotation(self, _mass, _ixxyyzz, _ixyxzyz, _unique=True):
Quaterniond(0.4, 0.2, 0.5),
Quaterniond(-0.1, 0.7, -0.7)]
for rot in rotations:
inertial = inertialRef
inertial = Inertiald(m, pose)
tol = -1e-6
self.assertTrue(inertial.set_mass_matrix_rotation(rot, tol))
self.assertEqual(moi, inertial.moi())
Expand All @@ -189,7 +189,7 @@ def SetRotation(self, _mass, _ixxyyzz, _ixyxzyz, _unique=True):
self.assertEqual(rot, inertial.pose().rot())
self.assertEqual(moi, inertial.moi())

inertial = inertialRef
inertial = Inertiald(m, pose)

self.assertTrue(inertial.set_inertial_rotation(rot))
self.assertEqual(rot, inertial.pose().rot())
Expand All @@ -214,23 +214,23 @@ def test_set_rotation_unique_non_diagonal(self):

def test_set_rotation_non_unique_diagonal(self):
self.SetRotation(12, Vector3d(2, 2, 2), Vector3d.ZERO, False)
# self.SetRotation(12, Vector3d(2, 2, 3), Vector3d.ZERO, False)
# self.SetRotation(12, Vector3d(2, 3, 2), Vector3d.ZERO, False)
self.SetRotation(12, Vector3d(2, 2, 3), Vector3d.ZERO, False)
self.SetRotation(12, Vector3d(2, 3, 2), Vector3d.ZERO, False)
self.SetRotation(12, Vector3d(3, 2, 2), Vector3d.ZERO, False)
# self.SetRotation(12, Vector3d(2, 3, 3), Vector3d.ZERO, False)
# self.SetRotation(12, Vector3d(3, 2, 3), Vector3d.ZERO, False)
# self.SetRotation(12, Vector3d(3, 3, 2), Vector3d.ZERO, False)

# def test_set_rotation_non_unique_non_diagonal(self):
# self.SetRotation(12, Vector3d(4, 4, 3), Vector3d(-1, 0, 0), False)
# self.SetRotation(12, Vector3d(4, 3, 4), Vector3d(0, -1, 0), False)
# self.SetRotation(12, Vector3d(3, 4, 4), Vector3d(0, 0, -1), False)
# self.SetRotation(12, Vector3d(4, 4, 5), Vector3d(-1, 0, 0), False)
# self.SetRotation(12, Vector3d(5, 4, 4), Vector3d(0, 0, -1), False)
# self.SetRotation(12, Vector3d(5.5, 4.125, 4.375),
# 0.25*Vector3d(-math.sqrt(3), 3.0, -math.sqrt(3)/2), False)
# self.SetRotation(12, Vector3d(4.125, 5.5, 4.375),
# 0.25*Vector3d(-math.sqrt(3), -math.sqrt(3)/2, 3.0), False)
self.SetRotation(12, Vector3d(2, 3, 3), Vector3d.ZERO, False)
self.SetRotation(12, Vector3d(3, 2, 3), Vector3d.ZERO, False)
self.SetRotation(12, Vector3d(3, 3, 2), Vector3d.ZERO, False)

def test_set_rotation_non_unique_non_diagonal(self):
self.SetRotation(12, Vector3d(4, 4, 3), Vector3d(-1, 0, 0), False)
self.SetRotation(12, Vector3d(4, 3, 4), Vector3d(0, -1, 0), False)
self.SetRotation(12, Vector3d(3, 4, 4), Vector3d(0, 0, -1), False)
self.SetRotation(12, Vector3d(4, 4, 5), Vector3d(-1, 0, 0), False)
self.SetRotation(12, Vector3d(5, 4, 4), Vector3d(0, 0, -1), False)
self.SetRotation(12, Vector3d(5.5, 4.125, 4.375),
Vector3d(-math.sqrt(3), 3.0, -math.sqrt(3)/2)*0.25, False)
self.SetRotation(12, Vector3d(4.125, 5.5, 4.375),
Vector3d(-math.sqrt(3), -math.sqrt(3)/2, 3.0)*0.25, False)

# test for diagonalizing MassMatrix
# verify MOI is conserved
Expand Down

0 comments on commit 60ceeea

Please sign in to comment.