def test_identity_initialization(self):
identity = CartesianState().Identity("test")
self.assertTrue(isinstance(identity, State))
self.assertFalse(identity.is_empty())
self.assertAlmostEqual(np.linalg.norm(identity.get_position()), 0)
self.assertAlmostEqual(np.linalg.norm(identity.get_orientation()), 1)
self.assertAlmostEqual(identity.get_orientation()[0], 1)
self.assertAlmostEqual(np.linalg.norm(identity.get_twist()), 0)
self.assertAlmostEqual(np.linalg.norm(identity.get_acceleration()), 0)
self.assertAlmostEqual(np.linalg.norm(identity.get_wrench()), 0)
def test_random_initialization(self):
random = CartesianState().Random("test")
self.assertTrue(isinstance(random, State))
self.assertFalse(random.is_empty())
self.assertTrue(np.linalg.norm(random.get_position()) > 0)
self.assertAlmostEqual(np.linalg.norm(random.get_orientation()), 1)
[self.assertTrue(random.get_orientation()[i] != 0) for i in range(4)]
self.assertTrue(np.linalg.norm(random.get_twist()) > 0)
self.assertTrue(np.linalg.norm(random.get_acceleration()) > 0)
self.assertTrue(np.linalg.norm(random.get_wrench()) > 0)
def test_norms(self):
cs = CartesianState().Random("test")
norms = cs.norms()
self.assertEqual(len(norms), 8)
self.assertAlmostEqual(norms[0], np.linalg.norm(cs.get_position()))
self.assertAlmostEqual(norms[1], np.linalg.norm(cs.get_orientation()))
self.assertAlmostEqual(norms[2],
np.linalg.norm(cs.get_linear_velocity()))
self.assertAlmostEqual(norms[3],
np.linalg.norm(cs.get_angular_velocity()))
self.assertAlmostEqual(norms[4],
np.linalg.norm(cs.get_linear_acceleration()))
self.assertAlmostEqual(norms[5],
np.linalg.norm(cs.get_angular_acceleration()))
self.assertAlmostEqual(norms[6], np.linalg.norm(cs.get_force()))
self.assertAlmostEqual(norms[7], np.linalg.norm(cs.get_torque()))
def test_get_set_fields(self):
cs = CartesianState("test")
# name
cs.set_name("robot")
self.assertEqual(cs.get_name(), "robot")
self.assertEqual(cs.get_reference_frame(), "world")
cs.set_reference_frame("base")
self.assertEqual(cs.get_reference_frame(), "base")
# position
position = [1., 2., 3.]
cs.set_position(position)
[
self.assertAlmostEqual(cs.get_position()[i], position[i])
for i in range(3)
]
cs.set_position(1.1, 2.2, 3.3)
assert_array_equal(np.array([1.1, 2.2, 3.3]), cs.get_position())
with self.assertRaises(RuntimeError):
cs.set_position([1., 2., 3., 4.])
# orientation
orientation_vec = np.random.rand(4)
orientation_vec = orientation_vec / np.linalg.norm(orientation_vec)
cs.set_orientation(orientation_vec)
[
self.assertAlmostEqual(cs.get_orientation()[i], orientation_vec[i])
for i in range(4)
]
# TODO what is an Eigen::Quaternion in Python ?
with self.assertRaises(RuntimeError):
cs.set_orientation(position)
matrix = cs.get_transformation_matrix()
trans = matrix[:3, 3]
# rot = matrix[:3, :3]
bottom = matrix[3, :]
assert_array_almost_equal(trans, cs.get_position())
# TODO rotation matrix from quaternion
assert_array_equal(bottom, np.array([0, 0, 0, 1]))
# pose
position = [4.4, 5.5, 6.6]
orientation_vec = np.random.rand(4)
orientation_vec = orientation_vec / np.linalg.norm(orientation_vec)
cs.set_pose(np.hstack((position, orientation_vec)))
assert_array_almost_equal(np.hstack((position, orientation_vec)),
cs.get_pose())
with self.assertRaises(RuntimeError):
cs.set_pose(position)
# twist
linear_velocity = np.random.rand(3)
cs.set_linear_velocity(linear_velocity)
assert_array_almost_equal(cs.get_linear_velocity(), linear_velocity)
angular_velocity = np.random.rand(3)
cs.set_angular_velocity(angular_velocity)
assert_array_almost_equal(cs.get_angular_velocity(), angular_velocity)
twist = np.random.rand(6)
cs.set_twist(twist)
assert_array_almost_equal(cs.get_twist(), twist)
# acceleration
linear_acceleration = np.random.rand(3)
cs.set_linear_acceleration(linear_acceleration)
assert_array_almost_equal(cs.get_linear_acceleration(),
linear_acceleration)
angular_acceleration = np.random.rand(3)
cs.set_angular_acceleration(angular_acceleration)
assert_array_almost_equal(cs.get_angular_acceleration(),
angular_acceleration)
acceleration = np.random.rand(6)
cs.set_acceleration(acceleration)
assert_array_almost_equal(cs.get_acceleration(), acceleration)
# wrench
force = np.random.rand(3)
cs.set_force(force)
assert_array_almost_equal(cs.get_force(), force)
torque = np.random.rand(3)
cs.set_torque(torque)
assert_array_almost_equal(cs.get_torque(), torque)
wrench = np.random.rand(6)
cs.set_wrench(wrench)
assert_array_almost_equal(cs.get_wrench(), wrench)
cs.set_zero()
self.assertAlmostEqual(np.linalg.norm(cs.data()), 1)
self.assertFalse(cs.is_empty())
cs.set_empty()
self.assertTrue(cs.is_empty())