def test_thruster_custom(self):
input_values = np.linspace(-50, 50, 10)
output_values = np.linspace(-10000, 10000, 10)
gain = 20000.0 / 100.0
# Use random positions and quaternions
for axis in AXES:
pos = np.random.rand(3)
q = transformations.random_quaternion()
thruster = Thruster.create_thruster(
self.node,
'custom',
IDX,
TOPIC,
pos,
q,
axis,
input=input_values,
output=output_values)
self.assertEqual(thruster.index, IDX)
self.assertEqual(thruster.topic, TOPIC)
self.assertTrue((thruster.tam_column == get_force_vector(pos, q, axis)).all())
self.assertTrue(np.isclose(thruster.get_thrust_value(0), 0))
self.assertTrue(np.isclose(thruster.get_command_value(0), 0))
x = random.random() * 10
self.assertTrue(np.isclose(thruster.get_thrust_value(x), gain * x))
y = random.random() * 10000
self.assertTrue(np.isclose(thruster.get_command_value(y), y / gain))
def test_thruster_proportional(self):
# Use random positions and quaternions
for axis in AXES:
pos = np.random.rand(3)
q = transformations.random_quaternion()
gain = random.random()
thruster = Thruster.create_thruster(
self.node,
'proportional',
IDX,
TOPIC,
pos,
q,
axis,
gain=gain)
self.assertEqual(thruster.index, IDX)
self.assertEqual(thruster.topic, TOPIC)
self.assertTrue((thruster.tam_column == get_force_vector(pos, q, axis)).all())
self.assertEqual(thruster.get_thrust_value(0), 0)
self.assertEqual(thruster.get_command_value(0), 0)
command = np.linspace(-100, 100, 10)
for x in command:
y = thruster.get_thrust_value(x)
self.assertEqual(y, gain * np.abs(x) * x)
thrust = np.linspace(-50000, 50000, 10)
for x in thrust:
y = thruster.get_command_value(x)
self.assertEqual(y, np.sign(x) * np.sqrt(np.abs(x) / gain))
def test_thruster(self):
# Use random positions and quaternions
for axis in AXES:
pos = np.random.rand(3)
q = transformations.random_quaternion()
thruster = Thruster(index=IDX,
topic=TOPIC,
pos=pos,
orientation=q,
axis=axis)
self.assertEqual(thruster.index, IDX)
self.assertEqual(thruster.topic, TOPIC)
self.assertTrue(
(thruster.tam_column == get_force_vector(pos, q, axis)).all())