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)