def setUp(self): self.finger = SimFinger( finger_type="single", 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 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_reproduce_reset_state(self): """ Send hundred states (positions + velocities) to all the 1DOF joints of the fingers and assert they exactly reach these states. """ finger = SimFinger(finger_type="fingerone") for _ in range(100): state_positions = sample.random_joint_positions( finger.number_of_fingers) state_velocities = [pos * 10 for pos in state_positions] reset_state = finger.reset_finger_positions_and_velocities( state_positions, state_velocities) reset_positions = reset_state.position reset_velocities = reset_state.velocity np.testing.assert_array_equal(reset_positions, state_positions, verbose=True) np.testing.assert_array_equal(reset_velocities, state_velocities, verbose=True)
def test_loading_urdfs_locally(self): """ Get the keys corresponding to the valid finger types from BaseFinger and try importing their corresponding URDFs. """ finger_data = finger_types_data.finger_types_data for key in finger_data.keys(): try: os.environ["ROS_PACKAGE_PATH"] = " " SimFinger(finger_type=key,) except pybullet.error as e: self.fail( "Failed to import the local copies of the urdf for" "SimFinger(finger_type={}): {}".format(key, e) )
def test_loading_urdfs(self): """ Get the keys corresponding to the valid finger types from BaseFinger and try importing their corresponding URDFs. """ finger_data = finger_types_data.finger_types_data for key in finger_data.keys(): try: SimFinger(finger_type=key,) except pybullet.error as e: self.fail( "Failed to create SimFinger(finger_type={}): {}".format( key, e ) )
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)
class TriFingerPlatform: """ Wrapper around the simulation providing the same interface as ``robot_interfaces::TriFingerPlatformFrontend``. The following methods of the robot_interfaces counterpart are not supported: - get_robot_status() - wait_until_timeindex() """ # Create the action and observation spaces # ======================================== _n_joints = 9 _n_fingers = 3 _max_torque_Nm = 0.36 _max_velocity_radps = 10 spaces = SimpleNamespace() spaces.robot_torque = SimpleNamespace( low=np.full(_n_joints, -_max_torque_Nm, dtype=np.float32), high=np.full(_n_joints, _max_torque_Nm, dtype=np.float32), default=np.zeros(_n_joints, dtype=np.float32), ) spaces.robot_position = SimpleNamespace( low=np.array([-0.9, -1.57, -2.7] * _n_fingers, dtype=np.float32), high=np.array([1.4, 1.57, 0.0] * _n_fingers, dtype=np.float32), default=np.array( [0.0, np.deg2rad(70), np.deg2rad(-130)] * _n_fingers, dtype=np.float32, ), ) spaces.robot_velocity = SimpleNamespace( low=np.full(_n_joints, -_max_velocity_radps, dtype=np.float32), high=np.full(_n_joints, _max_velocity_radps, dtype=np.float32), default=np.zeros(_n_joints, dtype=np.float32), ) spaces.object_position = SimpleNamespace( low=np.array([-0.3, -0.3, 0], dtype=np.float32), high=np.array([0.3, 0.3, 0.3], dtype=np.float32), default=np.array([0, 0, move_cube._min_height], dtype=np.float32), ) spaces.object_orientation = SimpleNamespace( low=-np.ones(4, dtype=np.float32), high=np.ones(4, dtype=np.float32), default=move_cube.Pose().orientation, ) # for convenience, we also create the respective gym spaces spaces.robot_torque.gym = gym.spaces.Box(low=spaces.robot_torque.low, high=spaces.robot_torque.high) spaces.robot_position.gym = gym.spaces.Box(low=spaces.robot_position.low, high=spaces.robot_position.high) spaces.robot_velocity.gym = gym.spaces.Box(low=spaces.robot_velocity.low, high=spaces.robot_velocity.high) spaces.object_position.gym = gym.spaces.Box( low=spaces.object_position.low, high=spaces.object_position.high) spaces.object_orientation.gym = gym.spaces.Box( low=spaces.object_orientation.low, high=spaces.object_orientation.high) def __init__( self, visualization=False, initial_robot_position=None, initial_object_pose=None, enable_cameras=False, ): """Initialize. Args: visualization (bool): Set to true to run visualization. initial_robot_position: Initial robot joint angles initial_object_pose: Initial pose for the manipulation object. Can be any object with attributes ``position`` (x, y, z) and ``orientation`` (x, y, z, w). This is optional, if not set, a random pose will be sampled. enable_cameras (bool): Set to true to enable camera observations. By default this is disabled as rendering of images takes a lot of computational power. Therefore the cameras should only be enabled if the images are actually used. """ #: Camera rate in frames per second. Observations of camera and #: object pose will only be updated with this rate. #: NOTE: This is currently not used! self._camera_rate_fps = 30 #: Set to true to render camera observations self.enable_cameras = enable_cameras #: Simulation time step self._time_step = 0.004 # first camera update in the first step self._next_camera_update_step = 0 # Initialize robot, object and cameras # ==================================== self.simfinger = SimFinger( finger_type="trifingerpro", time_step=self._time_step, enable_visualization=visualization, ) if initial_robot_position is None: initial_robot_position = self.spaces.robot_position.default self.simfinger.reset_finger_positions_and_velocities( initial_robot_position) if initial_object_pose is None: initial_object_pose = move_cube.Pose( position=self.spaces.object_position.default, orientation=self.spaces.object_orientation.default, ) self.cube = collision_objects.Block( initial_object_pose.position, initial_object_pose.orientation, mass=0.020, ) self.tricamera = camera.TriFingerCameras() # Forward some methods for convenience # ==================================== # forward "RobotFrontend" methods directly to simfinger self.Action = self.simfinger.Action self.get_desired_action = self.simfinger.get_desired_action self.get_applied_action = self.simfinger.get_applied_action self.get_timestamp_ms = self.simfinger.get_timestamp_ms self.get_current_timeindex = self.simfinger.get_current_timeindex self.get_robot_observation = self.simfinger.get_observation # forward kinematics directly to simfinger self.forward_kinematics = ( self.simfinger.pinocchio_utils.forward_kinematics) # Initialize log # ============== self._action_log = { "initial_robot_position": initial_robot_position.tolist(), "initial_object_pose": { "position": initial_object_pose.position.tolist(), "orientation": initial_object_pose.orientation.tolist(), }, "actions": [], } def get_time_step(self): """Get simulation time step in seconds.""" return self._time_step def _compute_camera_update_step_interval(self): return (1.0 / self._camera_rate_fps) / self._time_step def append_desired_action(self, action): """ Call :meth:`pybullet.SimFinger.append_desired_action` and add the action to the action log. Arguments/return value are the same as for :meth:`pybullet.SimFinger.append_desired_action`. """ # update camera and object observations only with the rate of the # cameras # next_t = self.get_current_timeindex() + 1 # has_camera_update = next_t >= self._next_camera_update_step # if has_camera_update: # self._next_camera_update_step += ( # self._compute_camera_update_step_interval() # ) # self._object_pose_t = self._get_current_object_pose() # if self.enable_cameras: # self._camera_observation_t = ( # self._get_current_camera_observation() # ) has_camera_update = True self._object_pose_t = self._get_current_object_pose() if self.enable_cameras: self._camera_observation_t = self._get_current_camera_observation() t = self.simfinger.append_desired_action(action) # The correct timestamp can only be acquired now that t is given. # Update it accordingly in the object and camera observations if has_camera_update: camera_timestamp_s = self.get_timestamp_ms(t) / 1000 self._object_pose_t.timestamp = camera_timestamp_s if self.enable_cameras: for i in range(len(self._camera_observation_t.cameras)): self._camera_observation_t.cameras[ i].timestamp = camera_timestamp_s # write the desired action to the log self._action_log["actions"].append({ "t": t, "torque": action.torque.tolist(), "position": action.position.tolist(), "position_kp": action.position_kp.tolist(), "position_kd": action.position_kd.tolist(), }) return t def _get_current_object_pose(self, t=None): cube_state = self.cube.get_state() pose = ObjectPose() pose.position = np.asarray(cube_state[0]) pose.orientation = np.asarray(cube_state[1]) pose.confidence = 1.0 # NOTE: The timestamp can only be set correctly after time step t is # actually reached. Therefore, this is set to None here and filled # with the proper value later. if t is None: pose.timestamp = None else: pose.timestamp = self.get_timestamp_ms(t) return pose def get_object_pose(self, t): """Get object pose at time step t. Args: t: The time index of the step for which the object pose is requested. Only the value returned by the last call of :meth:`~append_desired_action` is valid. Returns: ObjectPose: Pose of the object. Values come directly from the simulation without adding noise, so the confidence is 1.0. Raises: ValueError: If invalid time index ``t`` is passed. """ current_t = self.simfinger._t if t < 0: raise ValueError("Cannot access time index less than zero.") elif t == current_t: return self._object_pose_t elif t == current_t + 1: return self._get_current_object_pose(t) else: raise ValueError( "Given time index t has to match with index of the current" " step or the next one.") def _get_current_camera_observation(self, t=None): images = self.tricamera.get_images() observation = TriCameraObservation() # NOTE: The timestamp can only be set correctly after time step t # is actually reached. Therefore, this is set to None here and # filled with the proper value later. if t is None: timestamp = None else: timestamp = self.get_timestamp_ms(t) for i, image in enumerate(images): observation.cameras[i].image = image observation.cameras[i].timestamp = timestamp return observation def get_camera_observation(self, t): """Get camera observation at time step t. Args: t: The time index of the step for which the observation is requested. Only the value returned by the last call of :meth:`~append_desired_action` is valid. Returns: TriCameraObservation: Observations of the three cameras. Images are rendered in the simulation. Note that they are not optimized to look realistically. Raises: ValueError: If invalid time index ``t`` is passed. """ if not self.enable_cameras: raise RuntimeError( "Cameras are not enabled. Create `TriFingerPlatform` with" " `enable_cameras=True` if you want to use camera" " observations.") current_t = self.simfinger._t if t < 0: raise ValueError("Cannot access time index less than zero.") elif t == current_t: return self._camera_observation_t elif t == current_t + 1: return self._get_current_camera_observation(t) else: raise ValueError( "Given time index t has to match with index of the current" " step or the next one.") def store_action_log(self, filename): """Store the action log to a JSON file. Args: filename (str): Path to the JSON file to which the log shall be written. If the file exists already, it will be overwritten. """ # TODO should the log also contain intermediate observations (object # and finger) for verification? t = self.simfinger.get_current_timeindex() object_pose = self.get_object_pose(t) self._action_log["final_object_pose"] = { "position": object_pose.position.tolist(), "orientation": object_pose.orientation.tolist(), } with open(filename, "w") as fh: json.dump(self._action_log, fh)
def __init__( self, visualization=False, initial_robot_position=None, initial_object_pose=None, enable_cameras=False, ): """Initialize. Args: visualization (bool): Set to true to run visualization. initial_robot_position: Initial robot joint angles initial_object_pose: Initial pose for the manipulation object. Can be any object with attributes ``position`` (x, y, z) and ``orientation`` (x, y, z, w). This is optional, if not set, a random pose will be sampled. enable_cameras (bool): Set to true to enable camera observations. By default this is disabled as rendering of images takes a lot of computational power. Therefore the cameras should only be enabled if the images are actually used. """ #: Camera rate in frames per second. Observations of camera and #: object pose will only be updated with this rate. #: NOTE: This is currently not used! self._camera_rate_fps = 30 #: Set to true to render camera observations self.enable_cameras = enable_cameras #: Simulation time step self._time_step = 0.004 # first camera update in the first step self._next_camera_update_step = 0 # Initialize robot, object and cameras # ==================================== self.simfinger = SimFinger( finger_type="trifingerpro", time_step=self._time_step, enable_visualization=visualization, ) if initial_robot_position is None: initial_robot_position = self.spaces.robot_position.default self.simfinger.reset_finger_positions_and_velocities( initial_robot_position) if initial_object_pose is None: initial_object_pose = move_cube.Pose( position=self.spaces.object_position.default, orientation=self.spaces.object_orientation.default, ) self.cube = collision_objects.Block( initial_object_pose.position, initial_object_pose.orientation, mass=0.020, ) self.tricamera = camera.TriFingerCameras() # Forward some methods for convenience # ==================================== # forward "RobotFrontend" methods directly to simfinger self.Action = self.simfinger.Action self.get_desired_action = self.simfinger.get_desired_action self.get_applied_action = self.simfinger.get_applied_action self.get_timestamp_ms = self.simfinger.get_timestamp_ms self.get_current_timeindex = self.simfinger.get_current_timeindex self.get_robot_observation = self.simfinger.get_observation # forward kinematics directly to simfinger self.forward_kinematics = ( self.simfinger.pinocchio_utils.forward_kinematics) # Initialize log # ============== self._action_log = { "initial_robot_position": initial_robot_position.tolist(), "initial_object_pose": { "position": initial_object_pose.position.tolist(), "orientation": initial_object_pose.orientation.tolist(), }, "actions": [], }
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_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)