# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/index.html
# Note: It is not required to keep a copy of this file, your python script is saved with the station
from robolink import *    # RoboDK API
from robodk import *      # Robot toolbox
RDK = Robolink()


# Note:
# Plugin commands for the "CollisionFreePlanner" plugin are not case sensitive

# Example to retrieve the current PRM map information
status = RDK.PluginCommand("CollisionFreePlanner", "Info")
# It returns a string containing the samples and edges in the following format:
# "Samples-edges-Robot name"
print(status)

# Example to add the current robot joints (each joint will be connected to NewNodeEdges samples)
status = RDK.PluginCommand("CollisionFreePlanner", "Add")
print(status)
# This function can be called multiple times in a loop to create your own PRM map given a list of joint values

# Clear data related to the PRM collision free planner
status = RDK.PluginCommand("CollisionFreePlanner", "Clear")
print(status)

# Example to set the number of PRM samples (number of joint configurations):
status = RDK.PluginCommand("CollisionFreePlanner", "Samples", 25)
print(status)

# Example to set the number of PRM edges (connections between joint configurations):
status = RDK.PluginCommand("CollisionFreePlanner", "Edges", 10)
print(status)

# Example to set the number of edges for newly added joint samples
# (connections for new joint configurations added using the Add command or by joining targets/programs):
status = RDK.PluginCommand("CollisionFreePlanner", "NewNodeEdges", 5)
print(status)

# Example to change the path step for collision checking
# (this is a RoboDK command and not a PRM plugin command)
status = RDK.Command("PathStepCollisionDeg", 2)
print(status)

# Example to set the number of edges for newly added nodes
# (connections for new joint configurations added using the Add command or by joining targets/programs):
status = RDK.PluginCommand("CollisionFreePlanner", "Display", 0) #Set to 1 (display) or 0 (hidden)
print(status)

# Example to select a robot for PRM calculations (only useful if you have more than 2 robots in the cell)
robot_name = RDK.PluginCommand("CollisionFreePlanner", "SelectRobot", "Comau")
# It retuns the robot selected
print(robot_name)

# Calculate or update the PRM map (it can take a while)
status = RDK.PluginCommand("CollisionFreePlanner", "Calc")
print(status)


# Example to connect 2 robot targets (for example a program that moves from Target 1 to Target 2). The program will be called ProgSample
status = RDK.PluginCommand("CollisionFreePlanner", "Join=ProgSample", "Target 1|Target 2")
# Returns "Success" if it was possible to link both targets. Otherwise it returns "Failed"
print(status)

# Example to connect 2 programs (for example a program that moves from Program T1toT2 to T2toT1). The program will be called ProgJoined
status = RDK.PluginCommand("CollisionFreePlanner", "Join=ProgJoined", "T1toT2|T2toT1")
# Returns "Success" if it was possible to link both targets. Otherwise it returns "Failed"
print(status)


#--------------------------------------------------------
# Example to export the samples and the map as a CSV file
current_rdk_file = RDK.getParam("FILE_OPENSTATION")

# Save the N samples as a CSV file: joint values will be saved as one row per set of joints
status_msg = RDK.PluginCommand("CollisionFreePlanner", "SaveSamples", current_rdk_file + "-Joints.csv")
print(status_msg) # Returns "Done" if it worked

# Save the map as a CSV file (matrix of size NxN)
status_msg = RDK.PluginCommand("CollisionFreePlanner", "SaveMap", current_rdk_file + "-Map.csv")
print(status_msg) # Returns "Done" if it worked