def test_translation(self):
"""
Translation test.
"""
# params
expected_result = numpy.identity(4)
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
gripper_matrix = numpy.identity(4) # SDH transformation matrix
# No translation case
expected_result[2, 3] = offset
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result)
# Positive translation case
offset = 0.3 # meters
expected_result[2, 3] = offset
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result)
# Negative translation case
offset = -0.3 # meters
expected_result[2, 3] = offset
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result)
def calculate_poses_list(self,
target_pose,
sample_parameters,
number_of_fields=5):
"""
Calculates a list of poses around a target pose based on the spherical sampler parameters.
:param target_pose: The target pose.
:type target_pose: geometry_msgs.msg.PoseStamped()
:param sample_parameters: The parameters to specify a spherical sampling.
:type sample_parameters: mcr_manipulation_msgs.msg.SphericalSamplerParameters()
:param number_of_fields: The number of fields the SphericalSamplerParameters
message has.
:type number_of_fields: int
:return: A list of poses around the target pose.
:rtype: geometry_msgs.msg.PoseArray()
"""
poses = geometry_msgs.msg.PoseArray()
poses.header.frame_id = target_pose.header.frame_id
poses.header.stamp = target_pose.header.stamp
object_matrix = tf.transformations.quaternion_matrix([
target_pose.pose.orientation.x, target_pose.pose.orientation.y,
target_pose.pose.orientation.z, target_pose.pose.orientation.w
])
object_matrix[0, 3] = target_pose.pose.position.x
object_matrix[1, 3] = target_pose.pose.position.y
object_matrix[2, 3] = target_pose.pose.position.z
height_offsets = utils.generate_samples(
sample_parameters.height.minimum, sample_parameters.height.maximum,
self.linear_step, (self.max_samples / number_of_fields))
zeniths = utils.generate_samples(sample_parameters.zenith.minimum,
sample_parameters.zenith.maximum,
self.angular_step,
(self.max_samples / number_of_fields))
azimuths = utils.generate_samples(
sample_parameters.azimuth.minimum,
sample_parameters.azimuth.maximum, self.angular_step,
(self.max_samples / number_of_fields))
wrist_rolls = utils.generate_samples(
sample_parameters.yaw.minimum, sample_parameters.yaw.maximum,
self.angular_step, (self.max_samples / number_of_fields))
radials = utils.generate_samples(
sample_parameters.radial_distance.minimum,
sample_parameters.radial_distance.maximum, self.linear_step,
(self.max_samples / number_of_fields))
transforms = [
transformations.generate_grasp_matrix(object_matrix,
self.gripper_config_matrix,
height_offset, zenith,
azimuth, wrist_roll, radial)
for height_offset in height_offsets for zenith in zeniths
for azimuth in azimuths for wrist_roll in wrist_rolls
for radial in radials
]
for transform in transforms:
pose = utils.matrix_to_pose(transform)
poses.poses.append(pose)
return poses
def test_zenith(self):
"""
Zenith test.
"""
## No zenith, no translation case
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
gripper_matrix = numpy.identity(4) # SDH transformation matrix
expected_result = numpy.identity(4)
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result)
## Zenith and no translation cases
# Negative zenith params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = -math.pi / 2 # radians
expected_result = numpy.array([[0.0, 0.0, -1.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result)
# Negative zenith params
offset = 0.0 # meters
azimuthal = 0.0 # rads
zenith = -math.pi / 6 # rads
expected_result = numpy.array([[0.866, 0.0, -0.5, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.5, 0.0, 0.866, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Zenith and translation, but no azimuthal
offset = 0.3 # meters
azimuthal = 0.0 # rads
zenith = math.pi / 6 # rads
expected_result = numpy.array([[0.866, 0.0, 0.5, 0.0],
[0.0, 1.0, 0.0, 0.0],
[-0.5, 0.0, 0.866, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Zenith and azimuthal, but no translation
offset = 0.0 # meters
azimuthal = -math.pi / 6 # rads
zenith = math.pi / 2 # rads
expected_result = numpy.array([[0.0, -0.5, 0.866, 0.0],
[0.0, 0.866, 0.5, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Zenith, azimuthal, and translation
offset = 0.3 # meters
azimuthal = -math.pi / 6 # rads
zenith = math.pi / 2 # rads
expected_result = numpy.array([[0.0, -0.5, 0.866, 0.0],
[0.0, 0.866, 0.5, 0.0],
[-1.0, 0.0, 0.0, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
def test_gripper_matrix(self):
"""
Gripper configuration matrix test.
"""
## Only gripper matrix
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = gripper_matrix
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only gripper matrix and height offset
# params
offset = 0.3 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only gripper matrix and radial
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.2 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, -radial],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only gripper matrix, offset, and radial
# params
offset = 0.3 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.2 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, offset - radial],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only gripper matrix and azimuthal
# params
offset = 0.0 # meters
azimuthal = math.pi / 6 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([
[0.0, 1.0, 0.0, 0.0],
[-0.866, 0.0, -0.5, 0.0],
[-0.5, 0.0, 0.866, 0.0],
[0.0, 0.0, 0.0, 1.0],
])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only gripper matrix and zenith
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = -math.pi # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[0.0, -1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only gripper matrix and wrist roll
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = -math.pi / 2 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[-1.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Rotation tests (azimuthal, zenith and/or wrist_roll)
# params
offset = 0.0 # meters
azimuthal = math.pi / 6 # radians
zenith = math.pi / 3 # radians
wrist_roll = -math.pi / 2 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[-0.5, -0.433, 0.75, 0.0],
[0.0, -0.866, -0.5, 0.0],
[0.866, -0.25, 0.433, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Rotation and height offset tests
# params
offset = 0.3 # meters
azimuthal = math.pi / 6 # radians
zenith = math.pi / 3 # radians
wrist_roll = -math.pi / 2 # radians
radial = 0.0 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[-0.5, -0.433, 0.75, 0.0],
[0.0, -0.866, -0.5, 0.0],
[0.866, -0.25, 0.433, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## All parameters tests
# params
offset = 0.3 # meters
azimuthal = math.pi / 6 # radians
zenith = math.pi / 3 # radians
wrist_roll = -math.pi / 2 # radians
radial = 0.2 # meters
# youBot gripper (precision grasp)
gripper_matrix = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
expected_result = numpy.array([[-0.5, -0.433, 0.75, -0.15],
[0.0, -0.866, -0.5, 0.1],
[0.866, -0.25, 0.433, 0.213],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result,
expected_result,
decimal=3,
err_msg="REs: {0}\nExp: {1}".format(
result, expected_result))
def test_radial(self):
"""
Radial test.
"""
## No radial
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
gripper_matrix = numpy.identity(4) # SDH transformation matrix
expected_result = numpy.identity(4)
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only radial
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.3 # meters
expected_result = numpy.identity(4)
expected_result[2, 3] = -radial
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Only radial and height
# params
offset = 0.3 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.1 # meters
expected_result = numpy.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, offset - radial],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Radial and zenith
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = -math.pi / 2 # radians
wrist_roll = 0.0 # radians
radial = 0.1 # meters
expected_result = numpy.array([[0.0, 0.0, -1.0, radial],
[0.0, 1.0, 0.0, 0.0],
[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
def test_wrist_roll(self):
"""
Wrist roll test.
"""
## No wrist roll, no zenith, no azimuthal, and no translation case
# params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = 0.0 # radians
radial = 0.0 # meters
gripper_matrix = numpy.identity(4) # SDH transformation matrix
expected_result = numpy.identity(4)
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Wrist roll, but no zenith, azimuthal, nor translation cases
# Positive wrist roll params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = math.pi / 6 # radians
expected_result = numpy.array([[0.866, -0.5, 0.0, 0.0],
[0.5, 0.866, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
# Negative wrist roll params
offset = 0.0 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = -math.pi / 2 # radians
expected_result = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Wrist roll and translation, but no zenith, nor azimuthal cases
# Positive wrist roll params
offset = 0.3 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = math.pi / 6 # radians
expected_result = numpy.array([[0.866, -0.5, 0.0, 0.0],
[0.5, 0.866, 0.0, 0.0],
[0.0, 0.0, 1.0, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
# Negative wrist roll params
offset = 0.3 # meters
azimuthal = 0.0 # radians
zenith = 0.0 # radians
wrist_roll = -math.pi / 2 # radians
expected_result = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Wrist roll, azimuthal, and translation, but no zenith cases
# Positive wrist roll params
offset = 0.3 # meters
azimuthal = math.pi / 6 # radians
zenith = 0.0 # radians
wrist_roll = math.pi / 6 # radians
expected_result = numpy.array([[0.866, -0.5, 0.0, 0.0],
[0.433, 0.75, -0.5, 0.0],
[0.25, 0.433, 0.866, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
# Negative wrist roll params
offset = 0.3 # meters
azimuthal = math.pi / 6 # radians
zenith = 0.0 # radians
wrist_roll = -math.pi / 2 # radians
expected_result = numpy.array([[0.0, 1.0, 0.0, 0.0],
[-0.866, 0.0, -0.5, 0.0],
[-0.5, 0.0, 0.866, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
## Wrist roll, azimuthal, translation, and zenith cases
# Positive wrist roll params
offset = 0.3 # meters
azimuthal = math.pi / 6 # radians
zenith = math.pi / 2 # radians
wrist_roll = math.pi / 6 # radians
expected_result = numpy.array([[0.25, 0.433, 0.866, 0.0],
[0.433, 0.75, -0.5, 0.0],
[-0.866, 0.5, 0.0, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)
# Negative wrist roll params
offset = 0.3 # meters
azimuthal = math.pi / 6 # radians
zenith = math.pi / 2 # radians
wrist_roll = -math.pi / 2 # radians
expected_result = numpy.array([[-0.5, 0.0, 0.866, 0.0],
[-0.866, 0.0, -0.5, 0.0],
[0.0, -1.0, 0.0, offset],
[0.0, 0.0, 0.0, 1.0]])
result = transformations.generate_grasp_matrix(OBJECT_MATRIX,
gripper_matrix, offset,
zenith, azimuthal,
wrist_roll, radial)
testing.assert_almost_equal(result, expected_result, decimal=3)