def test_step_both(self):
# Create two instances and send different actions to them.
# Verify that both go towards their target
robot1 = SimFinger(finger_type="trifingerpro")
robot2 = SimFinger(finger_type="trifingerpro")
start_position = np.array([0.0, 0.7, -1.5] * 3)
robot1.reset_finger_positions_and_velocities(start_position)
robot2.reset_finger_positions_and_velocities(start_position)
action1 = robot1.Action(position=[0.5, 0.7, -1.5] * 3)
action2 = robot2.Action(position=[-0.5, 0.7, -1.5] * 3)
for i in range(1000):
t1 = robot1.append_desired_action(action1)
t2 = robot2.append_desired_action(action2)
obs1 = robot1.get_observation(t1)
obs2 = robot2.get_observation(t2)
if i > 1:
self.assertTrue((obs2.position != obs1.position).any())
self.assertLess(np.linalg.norm(action1.position - obs1.position), 0.1)
self.assertLess(np.linalg.norm(action2.position - obs2.position), 0.1)
def test_constant_torque(self):
"""Compare two runs sending a constant torque.
In each run the finger is reset to a fixed position and a constant
torque is applied for a number of steps. The final observation of each
run is compared. If the simulation behaves deterministically, the
observations should be equal.
"""
finger = SimFinger(finger_type="single",)
start_position = [0.5, -0.7, -1.5]
action = finger.Action(torque=[0.3, 0.3, 0.3])
def run():
finger.reset_finger_positions_and_velocities(start_position)
for i in range(30):
finger._set_desired_action(action)
finger._step_simulation()
return finger._get_latest_observation()
first_run = run()
second_run = run()
np.testing.assert_array_equal(first_run.torque, second_run.torque)
np.testing.assert_array_equal(first_run.position, second_run.position)
np.testing.assert_array_equal(first_run.velocity, second_run.velocity)
def execute_random_motion(*, finger_name, nb_timesteps, enable_visualization):
finger = SimFinger(
finger_type=finger_name,
time_step=0.004,
enable_visualization=enable_visualization,
)
cube = collision_objects.Block()
for t in range(nb_timesteps):
if t % 50 == 0:
torque = np.random.uniform(low=-0.36, high=0.36, size=(9))
finger.append_desired_action(finger.Action(torque=torque))
del finger
del cube
def test_step_one(self):
# Create two instances, send actions and step only one. Verify that
# the other one does not move.
robot1 = SimFinger(finger_type="trifingerpro")
robot2 = SimFinger(finger_type="trifingerpro")
start_position = np.array([0.0, 0.7, -1.5] * 3)
robot1.reset_finger_positions_and_velocities(start_position)
robot2.reset_finger_positions_and_velocities(start_position)
action = robot1.Action(torque=[0.3, 0.3, 0.3] * 3)
for i in range(30):
robot1._set_desired_action(action)
robot1._step_simulation()
obs1 = robot1._get_latest_observation()
obs2 = robot2._get_latest_observation()
self.assertTrue((start_position != obs1.position).any())
np.testing.assert_array_equal(start_position, obs2.position)
class TestRobotEquivalentInterface(unittest.TestCase):
"""
Test the methods of SimFinger that provide an interface equivalent to the
RobotFrontend of robot_interfaces.
"""
def setUp(self):
self.finger = SimFinger(
finger_type="fingerone",
time_step=0.001,
enable_visualization=False,
)
self.start_position = [0, -0.7, -1.5]
self.finger.reset_finger_positions_and_velocities(self.start_position)
def tearDown(self):
# destroy the simulation to ensure that the next test starts with a
# clean state
del self.finger
def test_timing_action_t_vs_observation_t(self):
"""Verify that observation_t is really not affected by action_t."""
# Apply a max torque action for one step
action = self.finger.Action(torque=[
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
])
t = self.finger.append_desired_action(action)
obs = self.finger.get_observation(t)
# as obs_t is from just before action_t is applied, the position should
# not yet have changed
np.testing.assert_array_equal(self.start_position, obs.position)
# after applying another action (even with zero torque), we should see
# the effect
t = self.finger.append_desired_action(self.finger.Action())
obs = self.finger.get_observation(t)
# new position should be less, as negative torque is applied
np.testing.assert_array_less(obs.position, self.start_position)
def test_timing_action_t_vs_observation_tplus1(self):
"""Verify that observation_{t+1} is affected by action_t."""
# Apply a max torque action for one step
action = self.finger.Action(torque=[
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
])
t = self.finger.append_desired_action(action)
obs = self.finger.get_observation(t + 1)
# new position should be less, as negative torque is applied
np.testing.assert_array_less(obs.position, self.start_position)
def test_timing_observation_t_vs_tplus1(self):
# Apply a max torque action for one step
action = self.finger.Action(torque=[
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
])
t = self.finger.append_desired_action(action)
obs_t = self.finger.get_observation(t)
obs_tplus1 = self.finger.get_observation(t + 1)
# newer position should be lesser, as negative torque is applied
np.testing.assert_array_less(obs_tplus1.position, obs_t.position)
def test_timing_observation_t_multiple_times(self):
"""
Verify that calling get_observation(t) multiple times always gives same
result.
"""
# Apply a max torque action for one step
action = self.finger.Action(torque=[
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
])
t = self.finger.append_desired_action(action)
obs_t1 = self.finger.get_observation(t)
# observation should not change when calling multiple times with same t
for i in range(10):
obs_ti = self.finger.get_observation(t)
np.testing.assert_array_equal(obs_t1.position, obs_ti.position)
np.testing.assert_array_equal(obs_t1.velocity, obs_ti.velocity)
np.testing.assert_array_equal(obs_t1.torque, obs_ti.torque)
np.testing.assert_array_equal(obs_t1.tip_force, obs_ti.tip_force)
def test_timing_observation_tplus1_multiple_times(self):
"""
Verify that calling get_observation(t + 1) multiple times always gives
same result.
"""
# Apply a max torque action for one step
action = self.finger.Action(torque=[
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
])
t = self.finger.append_desired_action(action)
obs_t1 = self.finger.get_observation(t + 1)
# observation should not change when calling multiple times with same t
for i in range(10):
obs_ti = self.finger.get_observation(t + 1)
np.testing.assert_array_equal(obs_t1.position, obs_ti.position)
np.testing.assert_array_equal(obs_t1.velocity, obs_ti.velocity)
np.testing.assert_array_equal(obs_t1.torque, obs_ti.torque)
np.testing.assert_array_equal(obs_t1.tip_force, obs_ti.tip_force)
def test_exception_on_old_t(self):
"""Verify that calling get_observation with invalid t raises error."""
# verify that t < 0 is not accepted
with self.assertRaises(ValueError):
self.finger.get_observation(-1)
# append two actions
t1 = self.finger.append_desired_action(self.finger.Action())
t2 = self.finger.append_desired_action(self.finger.Action())
# it should be possible to get observation for t2 and t2 + 1 but not t1
# or t2 + 2
self.finger.get_observation(t2)
self.finger.get_observation(t2 + 1)
with self.assertRaises(ValueError):
self.finger.get_observation(t1)
with self.assertRaises(ValueError):
self.finger.get_observation(t2 + 2)
def test_observation_types(self):
"""Verify that all fields of observation are np.ndarrays."""
# Apply a max torque action for one step
action = self.finger.Action(torque=[
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
-self.finger.max_motor_torque,
])
t = self.finger.append_desired_action(action)
obs = self.finger.get_observation(t)
# verify types
self.assertIsInstance(obs.torque, np.ndarray)
self.assertIsInstance(obs.position, np.ndarray)
self.assertIsInstance(obs.velocity, np.ndarray)
self.assertIsInstance(obs.tip_force, np.ndarray)
def test_get_desired_action(self):
# verify that t < 0 is not accepted
with self.assertRaises(ValueError):
self.finger.get_desired_action(-1)
orig_action = self.finger.Action(torque=[1.0, 2.0, 3.0],
position=[0.0, -1.0, -2.0])
t = self.finger.append_desired_action(orig_action)
action = self.finger.get_desired_action(t)
np.testing.assert_array_equal(orig_action.torque, action.torque)
np.testing.assert_array_equal(orig_action.position, action.position)
# verify types
self.assertIsInstance(action.torque, np.ndarray)
self.assertIsInstance(action.position, np.ndarray)
self.assertIsInstance(action.position_kd, np.ndarray)
self.assertIsInstance(action.position_kp, np.ndarray)
def test_get_applied_action(self):
# verify that t < 0 is not accepted
with self.assertRaises(ValueError):
self.finger.get_applied_action(-1)
desired_action = self.finger.Action(torque=[5, 5, 5])
t = self.finger.append_desired_action(desired_action)
applied_action = self.finger.get_applied_action(t)
np.testing.assert_array_almost_equal(
applied_action.torque,
[
self.finger.max_motor_torque,
self.finger.max_motor_torque,
self.finger.max_motor_torque,
],
)
# verify types
self.assertIsInstance(applied_action.torque, np.ndarray)
self.assertIsInstance(applied_action.position, np.ndarray)
self.assertIsInstance(applied_action.position_kd, np.ndarray)
self.assertIsInstance(applied_action.position_kp, np.ndarray)
def test_get_timestamp_ms_001(self):
t = self.finger.append_desired_action(self.finger.Action())
first_stamp = self.finger.get_timestamp_ms(t)
t = self.finger.append_desired_action(self.finger.Action())
second_stamp = self.finger.get_timestamp_ms(t)
# time step is set to 0.001, so the difference between two steps should
# be 1 ms.
self.assertEqual(second_stamp - first_stamp, 1)
def test_get_timestamp_ms_tplus1_001(self):
t = self.finger.append_desired_action(self.finger.Action())
stamp_t = self.finger.get_timestamp_ms(t)
stamp_tp1 = self.finger.get_timestamp_ms(t + 1)
# time step is set to 0.001, so the difference between two steps should
# be 1 ms.
self.assertEqual(stamp_tp1 - stamp_t, 1)
def test_get_timestamp_ms_invalid_t(self):
# verify that t < 0 is not accepted
with self.assertRaises(ValueError):
self.finger.get_timestamp_ms(-1)
t = self.finger.append_desired_action(self.finger.Action())
with self.assertRaises(ValueError):
self.finger.get_timestamp_ms(t - 1)
with self.assertRaises(ValueError):
self.finger.get_timestamp_ms(t + 2)
def test_get_current_timeindex(self):
# no time index before first action
with self.assertRaises(ValueError):
self.finger.get_current_timeindex()
t = self.finger.append_desired_action(self.finger.Action())
self.assertEqual(self.finger.get_current_timeindex(), t)