def test_below_y_axis(self):
above_pose = Pose()
above_pose.position.x = 0.0
above_pose.position.y = 0.0
above_pose.position.z = 0.0
above_pose.orientation.x = 0.0
above_pose.orientation.y = 0.0
above_pose.orientation.z = 0.0
above_pose.orientation.w = 1.0
# R: 0, P: 175, Y: 0
below_pose = Pose()
below_pose.position.x = 0.0
below_pose.position.y = 0.0
below_pose.position.z = 0.0
below_pose.orientation.x = 0.0
below_pose.orientation.y = 0.0
below_pose.orientation.z = 0.0
below_pose.orientation.w = -0.043616403098766035
threshold_distance = 25
result = over_under(above_pose,
below_pose,
threshold_distance,
axis='y')
self.assertEquals(result, 0)
def test_above_within_threshold_y_axis(self):
above_pose = Pose()
above_pose.position.x = 0.0
above_pose.position.y = 1.0
above_pose.position.z = 1.0
above_pose.orientation.x = 0.0
above_pose.orientation.y = 0.0
above_pose.orientation.z = 0.0
above_pose.orientation.w = 1.0
# R: 0, P: 175, Y: 0
below_pose = Pose()
below_pose.position.x = 0.0
below_pose.position.y = 0.0
below_pose.position.z = 0.0
below_pose.orientation.x = 0.0
below_pose.orientation.y = 0.0
below_pose.orientation.z = 0.0
below_pose.orientation.w = 1.0
threshold_distance = 2.5
result = over_under(above_pose,
below_pose,
threshold_distance,
axis='y')
self.assertEquals(result, 1)
def test_above_outside_threshold_x_axis(self):
above_pose = Pose()
above_pose.position.x = 1.0
above_pose.position.y = 3.0
above_pose.position.z = 2.0
above_pose.orientation.x = 0.0
above_pose.orientation.y = 0.0
above_pose.orientation.z = 0.0
above_pose.orientation.w = 1.0
# R: 0, P: 175, Y: 0
below_pose = Pose()
below_pose.position.x = 0.0
below_pose.position.y = 0.0
below_pose.position.z = 0.0
below_pose.orientation.x = 0.0
below_pose.orientation.y = 0.0
below_pose.orientation.z = 0.0
below_pose.orientation.w = 1.0
threshold_distance = 2.0
result = over_under(above_pose,
below_pose,
threshold_distance,
axis='x')
self.assertEquals(result, 0)
def evaluate(self, environment, observation):
"""
This function evaluates an observation for the assigned constraint of the class. It differentiates
between Sawyer (end-effector) and general items (blocks etc,.).
Parameters
----------
environment : Environment
The Environment object containing the current demonstrations environment (SawyerRobot, Items, Constraints)
and helper methods.
observation : Observation
The observation to evaluate for the constraint.
Returns
-------
: int
Integer value of constraint evaluation for the height constraint.
"""
if self.reference_pose is None:
above_pose, below_pose = self._get_poses(environment, observation)
else:
rospy.logdebug("Using reference pose")
above_pose, below_pose = self._get_poses_with_reference(
environment, observation)
return over_under(above_pose,
below_pose,
self.threshold_distance,
axis=self.axis)
def test_above_outside_threshold(self):
above_pose = Pose()
above_pose.position.x = 1.0
above_pose.position.y = 3.0
above_pose.position.z = 1.0
above_pose.orientation.x = 0.0
above_pose.orientation.y = 0.0
above_pose.orientation.z = 0.0
above_pose.orientation.w = 1.0
below_pose = Pose()
below_pose.position.x = 0.0
below_pose.position.y = 0.0
below_pose.position.z = 0.0
below_pose.orientation.x = 0.0
below_pose.orientation.y = 0.0
below_pose.orientation.z = 0.0
below_pose.orientation.w = 1.0
threshold_distance = 1.0
result = over_under(above_pose, below_pose, threshold_distance)
self.assertEquals(result, 0)
def test_below(self):
above_pose = Pose()
above_pose.position.x = 0.0
above_pose.position.y = 0.0
above_pose.position.z = 0.0
above_pose.orientation.x = 0.0
above_pose.orientation.y = 0.0
above_pose.orientation.z = 0.0
above_pose.orientation.w = 1.0
# R: 0, P: 175, Y: 0
below_pose = Pose()
below_pose.position.x = 0.0
below_pose.position.y = 0.0
below_pose.position.z = -1.0
below_pose.orientation.x = 0.0
below_pose.orientation.y = 0.0
below_pose.orientation.z = 0.0
below_pose.orientation.w = 1.0
threshold_distance = 1.0
result = over_under(above_pose, below_pose, threshold_distance)
self.assertEquals(result, 0)