def test_handler_validate_transform_mask():
"""Test."""
# test predesigned mask sequence
OptimizationHandler(robot=UR10(),
tool_mask=[False] * TRANSFORM_VECTOR_LENGTH)
# test error
with raises(PyboticsError):
OptimizationHandler(robot=UR10(), kinematic_chain_mask=[False])
def test_fk(resources_path: Path):
"""
Test robot.
:param robot:
:return:
"""
# get resource
path = resources_path / 'ur10-joints-poses.csv'
data = np.loadtxt(str(path), delimiter=',')
if data.ndim == 1:
data = np.expand_dims(data, axis=0)
# load robot
robot = UR10()
# test
for d in data:
joints = np.deg2rad(d[:robot.ndof])
desired_pose = d[robot.ndof:].reshape(TRANSFORM_MATRIX_SHAPE)
atol = 1e-3
# test with position argument
actual_pose = robot.fk(q=joints)
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)
def test_models():
"""Test."""
# test specific models
UR10(random_state=np.random.RandomState())
# iterate through all predefined models and init them
for _, o in inspect.getmembers(predefined_models, inspect.isclass):
if o.__module__ == predefined_models.__name__:
o()
def test_optimization():
"""Test."""
# init robot model and error wrt nominal
actual_robot = UR10()
actual_robot.tool.position = [0.1, 0, 0]
actual_robot.kinematic_chain.links[0].a += 0.1
# calculate fk
qs = np.tile(np.linspace(start=-np.pi, stop=np.pi, num=100),
(UR10.kinematic_chain.ndof, 1)).transpose()
poses = np.array(list(map(actual_robot.fk, qs)))
positions = poses[:, :-1, -1]
# init handler
handler = OptimizationHandler(robot=UR10(), )
handler.kinematic_chain_mask[1] = True
handler.tool_mask[0] = True
# run optimization
result = scipy.optimize.least_squares(
fun=optimize_accuracy,
x0=handler.generate_optimization_vector(),
args=(handler, qs, positions),
verbose=2) # type: scipy.optimize.OptimizeResult
# validate
atol = 1e-2
np.testing.assert_allclose(actual=result.x,
desired=handler.generate_optimization_vector(),
atol=atol)
np.testing.assert_allclose(actual=handler.robot.kinematic_chain.vector,
desired=actual_robot.kinematic_chain.vector,
atol=atol)
np.testing.assert_allclose(actual=handler.robot.tool.vector,
desired=actual_robot.tool.vector,
atol=atol)
np.testing.assert_allclose(actual=handler.robot.world_frame,
desired=actual_robot.world_frame,
atol=atol)
def test_joint_limits():
"""Test."""
robot = UR10()
# test setter
robot.joint_limits = robot.joint_limits.copy()
# test errors
with raises(PyboticsError):
robot.joint_limits = np.zeros(1)
with raises(PyboticsError):
robot.joints = robot.joint_limits.copy()[1] + 10
def test_compute_absolute_errors(q: np.ndarray):
"""Test."""
robot = UR10()
pose = robot.fk(q)
p = pose[:-1, -1]
# test 1D input
actual_error = compute_absolute_errors(qs=q, positions=p, robot=robot)
np.testing.assert_allclose(actual_error, 0)
# test 2D input
actual_error = compute_absolute_errors(qs=np.tile(q, (10, 1)),
positions=np.tile(p, (10, 1)),
robot=robot)
np.testing.assert_allclose(actual_error, 0)
def main():
"""
Simple function to test pybotics usage.
View source for more info.
"""
# init robot
robot = UR10()
# add tool
tool = Tool()
tool.position = [1, 2, 3]
robot.tool = tool
# set world frame
world_frame = vector_2_matrix([100, 200, 300, 0, 0, 0])
robot.world_frame = world_frame
# print debug info
print('Robot: {}'.format(robot))
print('Kinematic Chain: {}'.format(robot.kinematic_chain.to_dict()))
def test_ik(q: np.ndarray, q_offset: np.ndarray):
"""Test."""
robot = UR10()
pose = robot.fk(q)
# IK is hard to solve without a decent seed
q_actual = robot.ik(pose, q=robot.clamp_joints(q + q_offset))
if q_actual is None:
# ik couldn't be solved
# don't fail test
return
actual_pose = robot.fk(q_actual)
# 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)
# test the position with higher accuracy
desired_position = pose[:-1, -1]
actual_position = actual_pose[:-1, -1]
np.testing.assert_allclose(actual_position, desired_position, atol=1e-1)
def test_home_position():
"""Test."""
robot = UR10()
x = np.ones(len(robot))
robot.home_position = x
np.testing.assert_allclose(robot.home_position, x)
def test_str():
"""Test."""
str(UR10())
def test_len():
"""Test."""
len(UR10())
def test_repr():
"""Test."""
repr(UR10())
def test_random_joints():
"""Test."""
robot = UR10()
robot.random_joints()
robot.random_joints(in_place=True)