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
