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)