Skip to content

Commit

Permalink
fix: removed unused type: ignores
Browse files Browse the repository at this point in the history
  • Loading branch information
engnadeau committed Sep 21, 2022
1 parent 0df53f1 commit f7621bb
Show file tree
Hide file tree
Showing 7 changed files with 45 additions and 51 deletions.
2 changes: 1 addition & 1 deletion pybotics/geometry.py
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ def vector_2_matrix(
transform_matrix = np.eye(4)
for axis, value in zip(convention, rotation_component):
current_rotation = globals()[f"rotation_matrix_{axis}"](value)
transform_matrix = np.dot(transform_matrix, current_rotation) # type: ignore
transform_matrix = np.dot(transform_matrix, current_rotation)

# add translation component
transform_matrix[:-1, -1] = translation_component
Expand Down
14 changes: 7 additions & 7 deletions pybotics/optimization.py
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ def apply_optimization_vector(self, vector: npt.NDArray[np.float64]) -> None:
num_tool_parameters = np.sum(self.tool_mask)

# extract vector segments
segments = np.split( # type: ignore
segments = np.split(
vector, [num_kc_parameters, num_kc_parameters + num_tool_parameters]
)
kc_segment = segments[0]
Expand Down Expand Up @@ -107,7 +107,7 @@ def compute_absolute_error(
pose = robot.fk(q)
actual_position = position_from_matrix(pose)
error = position - actual_position # type: npt.NDArray[np.float64]
result = float(np.linalg.norm(error)) # type: ignore
result = float(np.linalg.norm(error))
return result


Expand All @@ -123,7 +123,7 @@ def compute_absolute_errors(
:param positions: Array of Cartesian positions, shape=(n-poses, 3)
:param robot: Robot model
"""
return np.fromiter( # type: ignore
return np.fromiter(
map(compute_absolute_error, qs, positions, repeat(robot)), dtype=np.float64
)

Expand All @@ -141,10 +141,10 @@ def compute_relative_error(
actual_position_a = position_from_matrix(pose_a)
actual_position_b = position_from_matrix(pose_b)

actual_distance = actual_position_a - actual_position_b # type: float
actual_distance = np.linalg.norm(actual_distance) # type: ignore
actual_distance = actual_position_a - actual_position_b
scalar_distance = np.linalg.norm(actual_distance)

error = float(np.linalg.norm(distance - actual_distance)) # type: ignore
error = float(np.linalg.norm(distance - scalar_distance))

return error

Expand All @@ -156,7 +156,7 @@ def compute_relative_errors(
robot: Robot,
) -> npt.NDArray[np.float64]:
"""Compute the relative errors of a given set of position combinations."""
return np.fromiter( # type: ignore
return np.fromiter(
map(compute_relative_error, qs_a, qs_b, distances, repeat(robot)),
dtype=np.float64,
)
12 changes: 5 additions & 7 deletions pybotics/robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ def fk(
# matrix multiply through transforms
pose = np.eye(4, dtype=float)
for t in transforms:
pose = np.dot(pose, t) # type: ignore
pose = np.dot(pose, t)

return pose

Expand Down Expand Up @@ -160,7 +160,7 @@ def jacobian_world(
j_tr = np.zeros((6, 6), dtype=float)
j_tr[:3, :3] = rotation
j_tr[3:, 3:] = rotation
j_w = np.dot(j_tr, j_fl) # type: ignore
j_w = np.dot(j_tr, j_fl)

return typing.cast(npt.NDArray[np.float64], j_w)

Expand Down Expand Up @@ -192,9 +192,7 @@ def jacobian_flange(
current_link = self.kinematic_chain.links[i]
p = q[i]
current_link_transform = current_link.transform(p)
current_transform = np.dot( # type: ignore
current_link_transform, current_transform
)
current_transform = np.dot(current_link_transform, current_transform)

return jacobian_flange

Expand Down Expand Up @@ -235,11 +233,11 @@ def compute_joint_torques(

# calculate force applied to current link
rotation = transform[:3, :3]
force = np.dot(rotation, force) # type: ignore
force = np.dot(rotation, force)

# calculate moment applied to current link
q = transform[:3, -1]
moment = np.dot(rotation, moment) + np.cross(q, force) # type: ignore
moment = np.dot(rotation, moment) + np.cross(q, force)

# append z-component as joint torque
joint_torques.append(moment[-1])
Expand Down
2 changes: 1 addition & 1 deletion tests/test_link.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ def test_len() -> None:
def test_displace() -> None:
"""Test."""
link = PrismaticMDHLink()
np.testing.assert_allclose(link.displace(), link.vector) # type: ignore
np.testing.assert_allclose(link.displace(), link.vector)


def test_repr() -> None:
Expand Down
36 changes: 18 additions & 18 deletions tests/test_optimization.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,16 +38,16 @@ def test_compute_absolute_errors(q: npt.NDArray[np.float64]) -> None:

# test 1D input; i.e., only one pose
actual_error = compute_absolute_error(q=q, position=position_vector, robot=robot)
np.testing.assert_allclose(actual_error, 0) # type: ignore
np.testing.assert_allclose(actual_error, 0)

# test 2D input; i.e., many poses
num_repeats = 10
actual_error = compute_absolute_errors( # type: ignore
qs=np.tile(q, (num_repeats, 1)), # type: ignore
positions=np.tile(position_vector, (num_repeats, 1)), # type: ignore
actual_error = compute_absolute_errors( # type: ignore
qs=np.tile(q, (num_repeats, 1)),
positions=np.tile(position_vector, (num_repeats, 1)),
robot=robot,
)
np.testing.assert_allclose(actual_error, 0) # type: ignore
np.testing.assert_allclose(actual_error, 0)


@given(
Expand All @@ -70,23 +70,23 @@ def test_compute_relative_errors(

p_a = robot.fk(q_a)[:-1, -1]
p_b = robot.fk(q_b)[:-1, -1]
distance = np.linalg.norm(p_a - p_b) # type: ignore
distance = np.linalg.norm(p_a - p_b)

# test 1D input; i.e., only one pose
actual_error = compute_relative_error(
q_a=q_a, q_b=q_b, distance=distance, robot=robot
)
np.testing.assert_allclose(actual_error, 0) # type: ignore
np.testing.assert_allclose(actual_error, 0)

# test 2D input; i.e., many poses
num_repeats = 10
actual_error = compute_relative_errors( # type: ignore
qs_a=np.tile(q_a, (num_repeats, 1)), # type: ignore
qs_b=np.tile(q_b, (num_repeats, 1)), # type: ignore
distances=np.tile(distance, (num_repeats, 1)), # type: ignore
actual_error = compute_relative_errors(
qs_a=np.tile(q_a, (num_repeats, 1)),
qs_b=np.tile(q_b, (num_repeats, 1)),
distances=np.tile(distance, (num_repeats, 1)),
robot=robot,
)
np.testing.assert_allclose(actual_error, 0) # type: ignore
np.testing.assert_allclose(actual_error, 0)


def test_optimization() -> None:
Expand All @@ -96,10 +96,10 @@ def test_optimization() -> None:
actual_robot.tool.position = np.array([0.1, 0, 0])
# FIXME: "Link" has no attribute "a"
# TODO: review abstract inheritance
actual_robot.kinematic_chain.links[0].a += 0.1 # type: ignore
actual_robot.kinematic_chain.links[0].a += 0.1

# calculate fk
qs = np.tile( # type: ignore
qs = np.tile(
np.linspace(start=-np.pi, stop=np.pi, num=100), (len(ur10()), 1)
).transpose()

Expand All @@ -121,18 +121,18 @@ def test_optimization() -> None:

# validate
atol = 1e-2
np.testing.assert_allclose( # type: ignore
np.testing.assert_allclose(
actual=result.x, desired=handler.generate_optimization_vector(), atol=atol
)
np.testing.assert_allclose( # type: ignore
np.testing.assert_allclose(
actual=handler.robot.kinematic_chain.vector,
desired=actual_robot.kinematic_chain.vector,
atol=atol,
)
np.testing.assert_allclose( # type: ignore
np.testing.assert_allclose(
actual=handler.robot.tool.vector, desired=actual_robot.tool.vector, atol=atol
)
np.testing.assert_allclose( # type: ignore
np.testing.assert_allclose(
actual=handler.robot.world_frame, desired=actual_robot.world_frame, atol=atol
)

Expand Down
22 changes: 10 additions & 12 deletions tests/test_robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ def test_fk() -> None:
data_path = (
Path(__file__).parent / "resources"
).resolve() / "ur10-joints-poses.csv"
data = np.loadtxt(str(data_path), delimiter=",") # type: ignore
data = np.loadtxt(str(data_path), delimiter=",")

# load robot
robot = Robot.from_parameters(ur10())
Expand All @@ -51,20 +51,20 @@ def test_fk() -> None:

# test with position argument
actual_pose = robot.fk(q=joints)
np.testing.assert_allclose(actual_pose, desired_pose, atol=atol) # type: ignore
np.testing.assert_allclose(actual_pose, desired_pose, atol=atol)

# test with internal position attribute
robot.joints = joints
actual_pose = robot.fk()
np.testing.assert_allclose(actual_pose, desired_pose, atol=atol) # type: ignore
np.testing.assert_allclose(actual_pose, desired_pose, atol=atol)


def test_home_position() -> None:
"""Test."""
robot = Robot.from_parameters(ur10())
x = np.ones(len(robot))
robot.home_position = x
np.testing.assert_allclose(robot.home_position, x) # type: ignore
np.testing.assert_allclose(robot.home_position, x)


def test_joint_limits() -> None:
Expand Down Expand Up @@ -115,11 +115,11 @@ def test_compute_joint_torques(planar_robot: Robot) -> None:

# test
actual_torques = planar_robot.compute_joint_torques(q=joint_angles, wrench=wrench)
np.testing.assert_allclose(actual_torques, expected_torques) # type: ignore
np.testing.assert_allclose(actual_torques, expected_torques)

planar_robot.joints = joint_angles
actual_torques = planar_robot.compute_joint_torques(wrench=wrench)
np.testing.assert_allclose(actual_torques, expected_torques) # type: ignore
np.testing.assert_allclose(actual_torques, expected_torques)


@given(
Expand Down Expand Up @@ -158,7 +158,7 @@ def test_jacobian_world(q: npt.NDArray[np.float64], planar_robot: Robot) -> None
expected[-1, :] = 1

actual = planar_robot.jacobian_world(q)
np.testing.assert_allclose(actual, expected, atol=1e-3) # type: ignore
np.testing.assert_allclose(actual, expected, atol=1e-3)


@given(
Expand Down Expand Up @@ -189,7 +189,7 @@ def test_jacobian_flange(q: npt.NDArray[np.float64], planar_robot: Robot) -> Non
expected[-1, :] = 1

actual = planar_robot.jacobian_flange(q)
np.testing.assert_allclose(actual, expected, atol=1e-6) # type: ignore
np.testing.assert_allclose(actual, expected, atol=1e-6)


@given(
Expand Down Expand Up @@ -229,14 +229,12 @@ def test_ik(q: npt.NDArray[np.float64], q_offset: npt.NDArray[np.float64]) -> No

# test the matrix with lower accuracy
# rotation components are hard to achieve when x0 isn't good
np.testing.assert_allclose(actual_pose, pose, atol=1) # type: ignore
np.testing.assert_allclose(actual_pose, pose, atol=1)

# test the position with higher accuracy
desired_position = pose[:-1, -1]
actual_position = actual_pose[:-1, -1]
np.testing.assert_allclose( # type: ignore
actual_position, desired_position, atol=1e-1
)
np.testing.assert_allclose(actual_position, desired_position, atol=1e-1)


def test_random_joints() -> None:
Expand Down
8 changes: 3 additions & 5 deletions tests/test_tool.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,9 @@ def test_tool() -> None:

cg = np.array([1, 2, 3])
tool.cg = cg
np.testing.assert_allclose(tool.cg, cg) # type: ignore
np.testing.assert_allclose(tool.cg, cg)

p = np.array([1, 2, 3])
tool.position = p
np.testing.assert_allclose(tool.position, p) # type: ignore
np.testing.assert_allclose( # type: ignore
tool.vector, matrix_2_vector(tool.matrix)
)
np.testing.assert_allclose(tool.position, p)
np.testing.assert_allclose(tool.vector, matrix_2_vector(tool.matrix))

0 comments on commit f7621bb

Please sign in to comment.