def test_tf_funcs(self):
v = Vector(1, 3, 2)
# Create from angle
tf = Transform(v, math.pi / 2)
self.assertAlmostEqual(tf.get_angle(), math.pi / 2)
# Extensively test the angle function:
for _ in range(0, 100):
angle = random.randrange(-100, 100)
self.assert_equal_angles(Transform(v, angle).get_angle(), angle)
g_tf = g_msgs.Transform()
g_tf.translation = v.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.geom_tf_almost_eq(g_tf, tf.to_geometry_msg())
# Should return 90 degrees
tf2 = Transform(Vector(1, 0, 0), Quaternion(1, 0, 0, 1).normalised)
self.assertEqual(tf2.get_angle(), math.pi / 2)
# Multiply transforms
self.assertEqual(tf * tf2, Transform(Vector(1, 4, 2), math.pi))
def interpolate_curvature(self, *, arc_length: float) -> float:
"""Interpolate the curvature at a given arc_length.
The curvature is approximated by calculating the Menger curvature as defined \
and described here:
https://en.wikipedia.org/wiki/Menger_curvature#Definition
Args:
arc_length (float): Length along the line starting from the first point
Raises:
ValueError: If the arc_length is <0 or more than the length of the line.
Returns:
Corresponding curvature.
"""
p = Vector(
self.interpolate(arc_length -
CURVATURE_APPROX_DISTANCE)) # Previous point
c = Vector(self.interpolate(arc_length)) # Point at current arc_length
n = Vector(self.interpolate(arc_length +
CURVATURE_APPROX_DISTANCE)) # Next point
# Area of the triangle spanned by p, c, and n.
# The triangle's area can be computed by the cross product of the vectors.
cross = (n - c).cross(p - c)
sign = 1 - 2 * (
cross.z < 0
) # The z-coord sign determines whether the curvature is positive or negative
return sign * 2 * abs(cross) / (abs(p - c) * abs(n - c) * abs(p - n))
def assert_approx_equal_pose(pose1, pose2):
"""Substitute self.assertAlmostEqual because pose - pose is not implemented."""
self.assertAlmostEqual(Vector(pose1.position),
Vector(pose2.position))
self.assertAlmostEqual(pose1.get_angle(),
pose2.get_angle(),
delta=math.radians(0.02))
def __init__(self, *args, frame=None):
"""Transform initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
# Attempt initialization from Vector like and Quaternion like objects
with suppress(Exception):
args = (args[0], Quaternion(*args[1]))
with suppress(Exception):
if isinstance(args[1], numbers.Number):
args = (args[0], Quaternion(axis=[0, 0, 1], radians=args[1]))
pass
# Attempt default init
with suppress(IndexError, NotImplementedError, TypeError):
if type(args[1]) == Quaternion:
self.rotation = args[1]
self.translation = Vector(args[0])
return
# Try to initialize geometry pose
with suppress(Exception):
# Call this function with values extracted
t = Vector(args[0].position)
g_quaternion = args[0].orientation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
# Try to initialize geometry transform
with suppress(Exception):
# Call this function with values extracted
t = Vector(args[0].translation)
g_quaternion = args[0].rotation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
with suppress(Exception):
# Try to initialize with two vectors translation+rotation
t = args[0]
rotation_vec = args[1].to_numpy()
angle = (-1 if rotation_vec[1] < 0 else 1) * math.acos(
np.dot([1, 0, 0], rotation_vec) / np.linalg.norm(rotation_vec))
self.__init__(t, angle)
return
# None of the initializations worked
raise NotImplementedError(
f"Transform initialization not implemented for {type(args[0])}")
def __init__(self, *args, frame=None):
"""Pose initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
with suppress(Exception):
args = (args[0], Quaternion(*args[1]))
with suppress(Exception):
if isinstance(args[1], numbers.Number):
args = (args[0], Quaternion(axis=[0, 0, 1], radians=args[1]))
# Attempt default init
with suppress(IndexError, NotImplementedError, TypeError):
if type(args[1]) == Quaternion:
self.position = Point(args[0])
self.orientation = args[1]
return
# Try to initialize geometry pose
with suppress(Exception):
# Call this function with values extracted
t = Point(args[0].position)
g_quaternion = args[0].orientation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
# Try to initialize geometry transform
with suppress(Exception):
# Call this function with values extracted
t = Point(args[0].translation)
g_quaternion = args[0].rotation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
with suppress(Exception):
# Try to initialize with two points translation+orientation
t = args[0]
orientation_vec = Vector(args[1])
angle = (-1 if orientation_vec.y < 0 else 1) * math.acos(
(Vector(1, 0, 0) * orientation_vec) / abs(orientation_vec))
self.__init__(t, angle)
return
# None of the initializations worked
raise NotImplementedError(
f"{self.__class__} initialization not implemented for {type(args[0])}"
)
def get_angle(self, axis=Vector(0, 0, 1)) -> float:
"""Angle of orientation.
Returns:
The angle that a vector is rotated, when this transformation is applied.
"""
# Project the rotation axis onto the z axis to get the amount of the rotation \
# that is in z direction!
# Also the quaternions rotation axis is sometimes (0,0,-1) at which point \
# the angles flip their sign,
# taking the scalar product of the axis and z fixes that as well
return Vector(self.orientation.axis) * axis * self.orientation.radians
def __rmul__(self, obj: Transform):
"""Right multiplication of a point. Only defined for transformations."""
if type(obj) == Transform:
# Transform * self
return self.__class__(obj * Vector(self))
raise InvalidPointOperationError("A point cannot be scaled.")
def get_angle(self, axis=Vector(0, 0, 1)) -> float:
"""Angle of rotation.
Args:
axis: Axis the rotation is projected onto.
Returns:
The angle that a vector is rotated, when this transformation is applied.
"""
# Project the rotation axis onto the rotation axis to get the amount of the rotation
# that is in the axis' direction!
# Also the quaternions rotation axis is sometimes flipped at which point
# the angles flip their sign,
# taking the scalar product with the axis fixes that as well
return Vector(self.rotation.axis) * axis * self.rotation.radians
def test_polygon_init(self):
points = self.create_points()
poly = Polygon(points)
# Check that polygon contains points
# Shapely adds first point at last index again
self.assertListEqual(poly.get_points()[:-1], points)
# Geometry msgs
g_polygon = g_msgs.Polygon()
g_polygon.points = [p.to_geometry_msg() for p in points]
poly_g = Polygon(g_polygon)
self.assertEqual(poly, poly_g)
# Numpy
array = np.array([p.to_numpy() for p in points])
poly_np = Polygon(array)
self.assertEqual(poly, poly_np)
# Check if polygon can be created correctly from array of two dimensional points
points_2d = [[2, 1], [3, 2], [34, 5], [3242, 1]]
points_3d = [[*p, 0] for p in points_2d]
self.assertEqual(Polygon(points_2d), Polygon(points_3d))
# Two lines
points_left = points
points_right = [(p + Vector(1, 0)) for p in points]
all_points = points_right.copy()
all_points.extend(reversed(points_left)) # Left side in reverse
poly1 = Polygon(Line(points_left), Line(points_right))
poly2 = Polygon(all_points)
self.assertEqual(poly1, poly2)
def test_tf_init(self):
"""Test if tf class can be initialized as expected."""
# Create from quaternion
v = Vector(1, 3, 2)
o = Quaternion(math.sqrt(1 / 2), 0, 0, math.sqrt(1 / 2))
tf = Transform(v, o)
# Create from angle
tf2 = Transform(v, math.pi / 2)
self.assertEqual(tf, tf2)
# Create from geometry_msgs/transform
g_tf = g_msgs.Transform()
g_tf.translation = v.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Transform(g_tf), tf)
def test_transformations(self):
"""Test if frame transformations work as expected.
Note: This is only tested for vectors because the behavior must be the same
for all classes that are transformable.
"""
frame_1 = Frame("frame_1")
frame_2 = Frame("frame_2")
frame_1.connect_to(frame_2,
transformation_to_frame=Transform([0, 0],
math.radians(90)))
vec_frame_1 = Vector(1, 0, 0, frame=frame_1)
vec_frame_2 = frame_2(vec_frame_1)
self.assertEqual(vec_frame_2, Vector(0, 1, 0, frame=frame_2))
vec_frame_2 = frame_2(vec_frame_2)
self.assertEqual(vec_frame_2, Vector(0, 1, 0, frame=frame_2))
def __mul__(self, tf: "Transform") -> "Transform":
"""Multiplication of transformations.
The product has to be understood as a single transformation consisting of
the right hand transformation applied first and then the left hand transformation.
Example:
Easily modify a vector multiple times:
:math:`(\\text{Tf}_1*\\text{Tf}_2)*\\vec{v} = \\text{Tf}_1*( \\text{Tf}_2*\\vec{v})`
Returns:
The product transformation.
"""
if tf.__class__ == self.__class__:
return self.__class__(
Vector(self) + Vector(tf).rotated(self.get_angle()),
self.get_angle() + tf.get_angle(),
)
return NotImplemented
def interpolate_direction(self, *, arc_length: float) -> Vector:
"""Interpolate the direction of the line as a vector.
Approximate by calculating difference vector of a point slightly further
and a point slightly before along the line.
Args:
arc_length (float): Length along the line starting from the first point
Raises:
ValueError: If the arc_length is <0 or more than the length of the line.
Returns:
Corresponding direction as a normalised vector.
"""
n = Vector(self.interpolate(arc_length + APPROXIMATION_DISTANCE))
p = Vector(self.interpolate(arc_length - APPROXIMATION_DISTANCE))
d = n - p
return 1 / abs(d) * d
def test_pose_funcs(self):
p = Point(1, 3, 2)
# Create from angle
pose = Pose(p, math.pi / 2)
self.assertAlmostEqual(pose.get_angle(), math.pi / 2)
g_pose = g_msgs.Pose()
g_pose.position = p.to_geometry_msg()
g_pose.orientation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.geom_pose_almost_eq(g_pose, pose.to_geometry_msg())
# Should return 90 degrees
pose2 = Pose(Point(1, 0, 0), Quaternion(-1, 0, 0, -1).normalised)
self.assertEqual(pose2.get_angle(), math.pi / 2)
# Apply transformations
tf2 = Transform(Vector(1, 0, 0), Quaternion(1, 0, 0, 1).normalised)
# Multiply transforms
self.assertEqual(tf2 * pose, Pose(Vector(1, 4, 2), math.pi))
def __rmul__(self, tf: "Transform"):
"""Apply transformation.
Args:
tf (Transform): Transformation to apply.
Returns:
Pose transformed by tf.
"""
with suppress(NotImplementedError, AttributeError):
return self.__class__(tf * Vector(self),
self.get_angle() + tf.get_angle())
return NotImplemented
def __rmul__(self, tf: "Transform"):
"""Apply transformation.
Args:
tf (Transform): Transformation to apply.
Returns:
Pose transformed by tf.
"""
with suppress(NotImplementedError, AttributeError):
return self.__class__(
tf * Vector(self.position),
tf.rotation * self.orientation,
frame=self._frame,
)
return NotImplemented
def pointcloud_cb(self, pc: PointCloud2):
"""Process new sensor information of depth camera."""
out_msg = Range()
out_msg.field_of_view = self.param.tof.field_of_view
out_msg.min_range = self.param.tof.min_range
out_msg.max_range = self.param.tof.max_range
out_msg.header.frame_id = self.param.frame_id
vecs = (Vector(p) for p in sensor_msgs.point_cloud2.read_points(
pc, field_names=("x", "y", "z")))
out_msg.range = min(v.norm for v in vecs)
rospy.logdebug(f"Pointcloud received in {rospy.get_name()}:{vecs}")
rospy.logdebug(f"Publishing range: {out_msg.range}")
self.publisher.publish(out_msg)
def test_point_func(self):
p1 = Point(1, 2, 3)
vec = Vector(2, 1, 3)
p3 = Point(3, 3, 6)
self.assertEqual(p1 + vec, p3)
self.assertEqual(p3 - vec, p1)
# The following should raise exception
with self.assertRaises(InvalidPointOperationError):
p1.rotated(0)
with self.assertRaises(InvalidPointOperationError):
p1 + p3
with self.assertRaises(InvalidPointOperationError):
p1 - p3
with self.assertRaises(InvalidPointOperationError):
3 * p3
def test_transformations(self):
tf = Transform(Vector(2, 2), math.pi / 2)
self.assertEqual(Vector(1, 3), tf * Vector(1, 1))
self.assertEqual(Point(1, 3), tf * Point(1, 1))
# Rotation around x-axis by 90 degrees
tf2 = Transform(Vector(0, 0),
Quaternion(axis=(1.0, 0.0, 0.0), radians=math.pi / 2))
self.assertEqual(Vector(1, 2, 3), tf2 * Vector(1, 3, -2))
# Check if line and polygon are transformed correctly
points = self.create_points()
transformed_points = [tf * p for p in points]
self.assertEqual(tf * Line(points), Line(transformed_points))
self.assertEqual(tf * Polygon(points), Polygon(transformed_points))
# Check to transform a pose
pose = Pose(Point(2, 2), math.pi)
self.assertEqual(tf * pose, Pose([0, 4], math.pi * 3 / 2))
class Transform:
"""Transformation class consisting of a translation and a rotation.
A Transform object can be used to easily interchange between multiple coordinate systems
(which can be transformed in one another via a rotation and a translation.
Initialization can be done in one of the following ways:
Args:
1 (geometry_msgs/Transformation): initialize from geometry_msgs.
2 (Vector, float): Second argument is the transformation's rotation angle in radian.
3 (Vector, pyquaternion.Quaternion): Vector and quaternion.
Attributes:
tranlation (Vector)
rotation (pyquaternion.Quaternion)
"""
def __init__(self, *args, frame=None):
"""Transform initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
# Attempt initialization from Vector like and Quaternion like objects
with suppress(Exception):
args = (args[0], Quaternion(*args[1]))
with suppress(Exception):
if isinstance(args[1], numbers.Number):
args = (args[0], Quaternion(axis=[0, 0, 1], radians=args[1]))
pass
# Attempt default init
with suppress(IndexError, NotImplementedError, TypeError):
if type(args[1]) == Quaternion:
self.rotation = args[1]
self.translation = Vector(args[0])
return
# Try to initialize geometry pose
with suppress(Exception):
# Call this function with values extracted
t = Vector(args[0].position)
g_quaternion = args[0].orientation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
# Try to initialize geometry transform
with suppress(Exception):
# Call this function with values extracted
t = Vector(args[0].translation)
g_quaternion = args[0].rotation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
with suppress(Exception):
# Try to initialize with two vectors translation+rotation
t = args[0]
rotation_vec = args[1].to_numpy()
angle = (-1 if rotation_vec[1] < 0 else 1) * math.acos(
np.dot([1, 0, 0], rotation_vec) / np.linalg.norm(rotation_vec))
self.__init__(t, angle)
return
# None of the initializations worked
raise NotImplementedError(
f"Transform initialization not implemented for {type(args[0])}")
@property
@validate_and_maintain_frames
def inverse(self) -> "Transform":
"""Inverse transformation."""
return Transform(
-1 * Vector(self.translation).rotated(self.rotation.inverse),
self.rotation.inverse,
)
def get_angle(self, axis=Vector(0, 0, 1)) -> float:
"""Angle of rotation.
Args:
axis: Axis the rotation is projected onto.
Returns:
The angle that a vector is rotated, when this transformation is applied.
"""
# Project the rotation axis onto the rotation axis to get the amount of the rotation
# that is in the axis' direction!
# Also the quaternions rotation axis is sometimes flipped at which point
# the angles flip their sign,
# taking the scalar product with the axis fixes that as well
return Vector(self.rotation.axis) * axis * self.rotation.radians
def to_geometry_msg(self) -> geometry_msgs.Transform:
"""Convert transform to ROS geometry_msg.
Returns:
This transformation as a geometry_msgs/Transform.
"""
vector = self.translation.to_geometry_msg()
rotation = geometry_msgs.Quaternion(self.rotation.x, self.rotation.y,
self.rotation.z, self.rotation.w)
tf = geometry_msgs.Transform()
tf.translation = vector
tf.rotation = rotation
return tf
def to_affine_matrix(self) -> np.ndarray:
"""Get transformation as an affine matrix."""
return np.column_stack((
self.rotation.rotation_matrix,
self.translation.to_numpy(),
))
@validate_and_maintain_frames
def __mul__(self, tf: "Transform") -> "Transform":
"""Multiplication of transformations.
The product has to be understood as a single transformation consisting of
the right hand transformation applied first and then the left hand transformation.
Example:
Easily modify a vector multiple times:
:math:`(\\text{Tf}_1*\\text{Tf}_2)*\\vec{v} =
\\text{Tf}_1*( \\text{Tf}_2*\\vec{v})`
Returns:
The product transformation.
"""
if tf.__class__ == self.__class__:
return self.__class__(
Vector(self.translation) +
Vector(tf.translation).rotated(self.rotation),
self.rotation * tf.rotation,
frame=self._frame,
)
return NotImplemented
@validate_and_maintain_frames
def __eq__(self, tf) -> bool:
if self.__class__ != tf.__class__:
return NotImplemented
return tf.rotation.normalised == self.rotation.normalised and Vector(
self.translation) == Vector(tf.translation)
def __repr__(self) -> str:
return (
f"Transform(translation={repr(self.translation)}," +
f"rotation={repr(self.rotation)}" +
(f",frame={self._frame.name}" if self._frame is not None else "") +
")")
def __hash__(self):
return NotImplemented
def inverse(self) -> "Transform":
"""Inverse transformation."""
return Transform(-1 * Vector(self).rotated(-self.get_angle()),
-1 * self.get_angle())
def test_line_interpolation_func(self):
line = Line([Point(0, 0), Point(1, 1)])
# Test if line direction function works
self.assertAlmostEqual(
line.interpolate_direction(arc_length=0),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length / 2),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(line.interpolate_curvature(arc_length=0),
0,
delta=TOLERANCE)
self.assertAlmostEqual(
line.interpolate_curvature(arc_length=line.length / 4),
0,
delta=TOLERANCE)
self.assertEqual(line.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector(1, 1, 0)))
circle = Line(
reversed([
p for p in Point(-1, 0).buffer(1,
resolution=4096).exterior.coords
]))
# Test if line direction function works
self.assertAlmostEqual(circle.interpolate_direction(arc_length=0),
Vector(0, 1),
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 2),
Vector(0, -1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length),
Vector(0, 1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length * 3 / 4),
Vector(1, 0))
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 4),
Vector(-1, 0))
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 2),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 4),
1,
delta=TOLERANCE)
def assert_approx_equal_pose(pose1, pose2):
self.assertAlmostEqual(Vector(pose1), Vector(pose2))
self.assertAlmostEqual(pose1.get_angle(),
pose2.get_angle(),
delta=math.radians(0.02))
assert_approx_equal_pose(circle.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector([0, 1, 0])))
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 4),
Pose(Point(-1, 1), Vector([-1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 2),
Pose(Point(-2, 0), Vector([0, -1, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length * 3 / 4),
Pose(Point(-1, -1), Vector([1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length),
Pose(Point(0, 0), Vector([0, 1, 0])),
)
def __eq__(self, tf) -> bool:
if self.__class__ != tf.__class__:
return NotImplemented
return tf.rotation.normalised == self.rotation.normalised and Vector(
self.translation) == Vector(tf.translation)
def assert_approx_equal_pose(pose1, pose2):
self.assertAlmostEqual(Vector(pose1), Vector(pose2))
self.assertAlmostEqual(pose1.get_angle(),
pose2.get_angle(),
delta=math.radians(0.02))
def inverse(self) -> "Transform":
"""Inverse transformation."""
return Transform(
-1 * Vector(self.translation).rotated(self.rotation.inverse),
self.rotation.inverse,
)
def test_line_interpolation_func(self):
line = Line([Point(0, 0), Point(1, 1)])
# Test if line direction function works
self.assertAlmostEqual(
line.interpolate_direction(arc_length=0),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length / 2),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(line.interpolate_curvature(arc_length=0),
0,
delta=TOLERANCE)
self.assertAlmostEqual(
line.interpolate_curvature(arc_length=line.length / 4),
0,
delta=TOLERANCE)
self.assertEqual(line.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector(1, 1, 0)))
circle = Line(
reversed([
p for p in Point(-1, 0).buffer(1,
resolution=4096).exterior.coords
]))
# Test if line direction function works
self.assertAlmostEqual(circle.interpolate_direction(arc_length=0),
Vector(0, 1),
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 2),
Vector(0, -1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length),
Vector(0, 1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length * 3 / 4),
Vector(1, 0))
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 4),
Vector(-1, 0))
self.assertAlmostEqual(circle.interpolate_curvature(arc_length=0),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 2),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 4),
1,
delta=TOLERANCE)
short_line = Line([
[0, 0],
[line_module.CURVATURE_APPROX_DISTANCE * 0.5, 0],
[
line_module.CURVATURE_APPROX_DISTANCE * 0.5,
line_module.CURVATURE_APPROX_DISTANCE * 0.5,
],
])
with self.assertRaises(ValueError):
short_line.interpolate_curvature(0)
just_long_enough_line = Line(
[[0, 0], [2 * line_module.CURVATURE_APPROX_DISTANCE, 0]])
self.assertEqual(just_long_enough_line.interpolate_curvature(0), 0)
def assert_approx_equal_pose(pose1, pose2):
"""Substitute self.assertAlmostEqual because pose - pose is not implemented."""
self.assertAlmostEqual(Vector(pose1.position),
Vector(pose2.position))
self.assertAlmostEqual(pose1.get_angle(),
pose2.get_angle(),
delta=math.radians(0.02))
assert_approx_equal_pose(circle.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector([0, 1, 0])))
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 4),
Pose(Point(-1, 1), Vector([-1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 2),
Pose(Point(-2, 0), Vector([0, -1, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length * 3 / 4),
Pose(Point(-1, -1), Vector([1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length),
Pose(Point(0, 0), Vector([0, 1, 0])),
)
