class TestProximitySensors(TestCore): def setUp(self): super().setUp() self.sensor = ProximitySensor('proximity_sensor') def test_read(self): self.pyrep.step() distance, object = self.sensor.read() self.pyrep.step() self.assertAlmostEqual(distance, 0.1) def test_is_detected(self): ob1 = Shape('simple_model') ob2 = Shape('dynamic_cube') self.assertTrue(self.sensor.is_detected(ob1)) self.assertFalse(self.sensor.is_detected(ob2))
class Gripper(RobotComponent): """Represents all types of end-effectors, e.g. grippers. """ def __init__(self, count: int, name: str, joint_names: List[str]): super().__init__(count, name, joint_names) suffix = '' if count == 0 else '#%d' % (count - 1) prox_name = '%s_attachProxSensor%s' % (name, suffix) attach_name = '%s_attachPoint%s' % (name, suffix) self._proximity_sensor = ProximitySensor(prox_name) self._attach_point = ForceSensor(attach_name) self._old_parents: List[Object] = [] self._grasped_objects: List[Object] = [] self._prev_positions = [None] * len(joint_names) self._prev_vels = [None] * len(joint_names) # Used to stop oscillating self._touch_sensors = [] i = 0 while True: fname = '%s_touchSensor%d%s' % (name, i, suffix) if not ForceSensor.exists(fname): break self._touch_sensors.append(ForceSensor(fname)) i += 1 def grasp(self, obj: Object) -> bool: """Grasp object if it is detected. Note: The does not actuate the gripper, but instead simply attaches the detected object to the gripper to 'fake' a grasp. :param obj: The object to grasp if detected. :return: True if the object was detected/grasped. """ detected = self._proximity_sensor.is_detected(obj) # Check if detected and that we are not already grasping it. if detected and obj not in self._grasped_objects: self._grasped_objects.append(obj) self._old_parents.append(obj.get_parent()) # type: ignore obj.set_parent(self._attach_point, keep_in_place=True) return detected def release(self) -> None: """Release any grasped objects. Note: The does not actuate the gripper, but instead simply detaches any grasped objects. """ for grasped_obj, old_parent in zip(self._grasped_objects, self._old_parents): # Check if the object still exists if grasped_obj.still_exists(): grasped_obj.set_parent(old_parent, keep_in_place=True) self._grasped_objects = [] self._old_parents = [] def get_grasped_objects(self) -> List[Object]: """Gets the objects that are currently grasped. :return: A list of grasped objects. """ return self._grasped_objects def actuate(self, amount: float, velocity: float) -> bool: """Actuate the gripper, but return after each simulation step. The functions attempts to open/close the gripper according to 'amount', where 1 represents open, and 0 represents close. The user should iteratively call this function until it returns True. This is a convenience method. If you would like direct control of the gripper, use the :py:class:`RobotComponent` methods instead. For some grippers, this method will need to be overridden. :param amount: A float between 0 and 1 representing the gripper open state. 1 means open, whilst 0 means closed. :param velocity: The velocity to apply to the gripper joints. :raises: ValueError if 'amount' is not between 0 and 1. :return: True if the gripper has reached its open/closed limits, or if the 'max_force' has been exerted. """ if not (0.0 <= amount <= 1.0): raise ValueError("'open_amount' should be between 0 and 1.'") _, joint_intervals_list = self.get_joint_intervals() joint_intervals = np.array(joint_intervals_list) # Decide on if we need to open or close joint_range = joint_intervals[:, 1] - joint_intervals[:, 0] target_pos = joint_intervals[:, 0] + (joint_range * amount) current_positions = self.get_joint_positions() done = True for i, (j, target, cur, prev) in enumerate( zip(self.joints, target_pos, current_positions, self._prev_positions)): # Check if the joint has moved much not_moving = (prev is not None and np.fabs(cur - prev) < POSITION_ERROR) reached_target = np.fabs(target - cur) < POSITION_ERROR vel = -velocity if cur - target > 0 else velocity oscillating = (self._prev_vels[i] is not None and vel != self._prev_vels[i]) if not_moving or reached_target or oscillating: j.set_joint_target_velocity(0) continue done = False self._prev_vels[i] = vel # type: ignore j.set_joint_target_velocity(vel) self._prev_positions = current_positions # type: ignore if done: self._prev_positions = [None] * self._num_joints self._prev_vels = [None] * self._num_joints self.set_joint_target_velocities([0.0] * self._num_joints) return done def get_open_amount(self) -> List[float]: """Gets the gripper open state. 1 means open, whilst 0 means closed. :return: A list of floats between 0 and 1 representing the gripper open state for each joint. 1 means open, whilst 0 means closed. """ _, joint_intervals_list = self.get_joint_intervals() joint_intervals = np.array(joint_intervals_list) joint_range = joint_intervals[:, 1] - joint_intervals[:, 0] return list( np.clip( (np.array(self.get_joint_positions()) - joint_intervals[:, 0]) / joint_range, 0.0, 1.0)) def get_touch_sensor_forces(self) -> List[List[float]]: if len(self._touch_sensors) == 0: raise NotImplementedError('No touch sensors for this robot!') return [ts.read()[0] for ts in self._touch_sensors] def get_object_in_hand(self): dist, object_handle = self._proximity_sensor.read() return dist, object_handle