# Bug example. MoveL_Test
# RoboDK
"""What I get when I run this script: the MoveL_Test returns -2 which means movement between jointVector0 and
jointVector1 should not be possible. However when I try to move from jointVector0 and jointVector1 with linear motion
the robot does the motion and no exceptions are raised."""

from robodk import robolink    # RoboDK API
from robodk import robomath    # Robot toolbox

USE_EXISTING_GUI = True

# Link to RoboDK
if USE_EXISTING_GUI:
    RDK = robolink.Robolink()
else:
    RDK = robolink.Robolink(args=['/NOSPLASH', '/NOSHOW'], quit_on_close=True)
# Select a robot (popup is displayed if more than one robot is available)
robot = RDK.ItemUserPick('Select a robot', robolink.ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception('No robot selected or available')

robot.setPoseFrame(robot.PoseFrame())
robot.setPoseTool(robot.PoseTool())
robot.setRounding(5)
robot.setSpeed(200) 
robot_joints = robot.Joints()
robot_position = robot.SolveFK(robot_joints)

Myframe = RDK.Item('TunnelFrame') 

# These are joint values for 2 different targets
jointVector0 = [-2.673, 11.398, 0.966, -276.009, -88.591, -167.432]
jointVector1 = [-1.381, 10.534, -14.661, -277.682, -86.983, 176.095]

test = robot.MoveL_Test(j1=robomath.Mat(jointVector0), pose=robot.SolveFK(robomath.Mat(jointVector1)))

print('test: ' + str(test))

RDK.setSimulationSpeed(1.0)

# Move from jointVector0 and jointVector1 with linear motion
robot.setJoints(robomath.Mat(jointVector0))
print("Performing linear motion between jointVector0 and jointVector1. Should not be possible according to MoveL_Test.")
robot.MoveL(robomath.Mat(jointVector1))