The Python interface
This section shows the Python interface to the services offered by the kautham_ros package in the file kautham_ros_interface_python.py.
Functions
Problem functions
Function kOpenProblem: Function that wraps the kautham_node/OpenProblem service (see OpenProblem)
Function kCloseProblem: Function that wraps the kautham_node/CloseProblem service (see CloseProblem)
Robot functions
Function kMoveRobot: Function that wraps the kautham_node/SetRobotsConfig service (see SetRobotsConfig)
Function kUpdateRobotDOFOffsetByName: Function that wraps the kautham_node/UpdateRobotDOFOffsetByName service (see UpdateRobotDOFOffsetByName)
Function kSetRobControlsNoQuery: Function that wraps the kautham_node/SetRobControlsNoQuery service (see SetRobotControlsNoQuery)
Function kGetRobotPos: Function that wraps the kautham_node/GetRobotPos service (see SetRobotPos / GetRobPos)
Function kSetRobotPos: Function that wraps the kautham_node/SetRobotPos service (see SetRobotPos / GetRobPos)
Obstacles functions
Function kAddObstacle: Function that wraps the kautham_node/AddObstacle service (see AddObstacle)
Function kRemoveObstacle: Function that wraps the kautham_node/RemoveObstacle service (see RemoveObstacle)
Function kAttachObject: Function that wraps the kautham_node/AttachObstacle2RobotLink service (see AttachObstacle2RobotLink)
Function kDetachObject: Function that wraps the kautham_node/DetachObstacle service (see DetachObstacle)
Function kSetObstaclePos: Function that wraps the kautham_node/SetObstaclePos service (see SetObstaclePos / GetObstaclePos)
Function kGetObstaclePos: Function that wraps the kautham_node/GetObstaclePos service (see SetObstaclePos / GetObstaclePos)
Planner functions
Function kSetPlannerParameter: Function that wraps the kautham_node/SetPlannerParameter service (see SetPlannerParameter)
Function kSetPlannerByName: Function that wraps the kautham_node/SetPlannerByName service (see SetPlannerByName)
Function kIsCollisionFree: Function that wraps the kautham_node/CheckCollision service (see CheckCollision)
Function kSolve: Function that wraps the kautham_node/Solve service (see Solve)
Function kSetControlQuery: Function that wraps the kautham_node/SetQueryByControlName service (see SetControlsQuery)
Function kSetQuery: Function that wraps the kautham_node/SetQuery service (see SetQuery [DEPRECATED, USE SetControlsQuery-label])
Function kGetPath: Function that wraps the kautham_node/GetPath service (see GetPath)
Function kPathDofNames: Function that wraps the kautham_node/PathDofNames service (see PathDofNames)
Function kGetTrajectory: Function that wraps the kautham_node/GetTrajectory service (see GetTrajectory)
Visualization functions
Function kVisualizeKauthamRVIZ: Function that wraps the kautham_node/KauthamRVIZ service (see KauthamRVIZ)
Function kVisualizeSampleRVIZ: Function that wraps the kautham_node/VisualizeSampleRVIZ service (see VisualizeSampleRVIZ)
An example
#!/usr/bin/env python3
import rclpy
import kautham_ros.kautham_ros_interface_python as kautham
import xml.etree.ElementTree as ET
import sys
def main():
rclpy.init(args=None)
node = rclpy.create_node('kautham_ros_client_python_node')
# Open config file:
root_demo_folder = "/usr/share/kautham"
# root_demo_folder = "/usr/local/share/kautham"
configfile = root_demo_folder+"/demos/OMPL_geometric_demos/chess_ur3_robotiq/kthconfig.xml"
modelFolder = root_demo_folder+"/demos/models/"
# heck for arguments:
args = sys.argv[1:] # args is a list of the command line args
if len(args)==1 or len(args)>2:
print("Erroneous number of args")
print("Use two arguments to define the root_demo_folder and the ktconfig file, e.g. ")
print(" $./kautham_ros_client_python_node.py /usr/local/share/kautham /demos/OMPL_geometric_demos/chess_ur3_robotiq/kthconfig.xml")
print("No arguments required to used the default values: /usr/share/kautham and /demos/OMPL_geometric_demos/chess_ur3_robotiq/kthconfig.xml")
return
if len(args)==2:
root_demo_folder = args[0]
configfile = root_demo_folder+args[1]
modelFolder = root_demo_folder+"/models/"
print("\n++++++++++++++++++++++++++++++++++++++")
print("Using root_demo_folder:", root_demo_folder)
print("Using kthconfig file:", configfile)
print("You can change them by passing arguments, e.g. ")
print(" $./kautham_ros_client_python_node.py /usr/local/share/kautham /demos/OMPL_geometric_demos/chess_ur3_robotiq/kthconfig.xml")
#Problem file:
config_tree = ET.parse(configfile)
config_root = config_tree.getroot()
kauthamproblem = config_root.find('Problemfiles').find('kautham').get('name')
kthconfig_directory =config_root.find('Problemfiles').find('directory').get('name')
print("\n++++++++++++++++++++++++++++++++++++++")
print("Using kautham problem defined in the kthconfig file: ",kauthamproblem)
#Setting problem files
root_problem_folder = root_demo_folder + kthconfig_directory
kauthamProblemFile = root_problem_folder + kauthamproblem
node.get_logger().info(
'Kautham Problem File: %s' %
(kauthamProblemFile))
#Open kautham problem
print("\n***************************************")
print(" Opening problem ")
print("***************************************")
kautham.kOpenProblem(node,modelFolder, kauthamProblemFile)
#Solve kautham problem
print("\n***************************************")
print(" Solving problem ")
print("***************************************")
kautham.kSolve(node)
print("\n***************************************")
print(" Get Path ")
print("***************************************")
path=kautham.kGetPath(node,True,0)
print("\n***************************************")
print(" Get Trajectory ")
print("***************************************")
traj = kautham.kGetTrajectory(node, True)
print("\n***************************************")
print(" Set query ")
print("***************************************")
init = (1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5)
goal = (0.662338, 0.271613, 0.760218, 0.722817, 0.738732, 0.659155, 0.676090, 0.643239, 0.213521, 0.770563, 0.245352, 0.261268, 0.500000, 0.600000)
print("Init = ", init)
print("Goal = ", goal)
path=kautham.kSetQuery(node,init,goal)
print("\n***************************************")
print(" Set Planner and planner parameters ")
print("***************************************")
print("Setting omplRRTConnect planner with Range 0.7")
kautham.kSetPlannerByName(node, "omplRRTConnect")
kautham.kSetPlannerParameter(node, "Range", "0.7")
#Solve kautham problem
print("\n***************************************")
print(" Solving problem Again ")
print("***************************************")
kautham.kSolve(node)
print("\n***************************************")
print(" Get Path Again ")
print("***************************************")
path=kautham.kGetPath(node,False,0)
print("\n***************************************")
print(" Check DOF names ")
print("***************************************")
dofnames = kautham.kPathDofNames(node)
print(dofnames)
print("\n***********************************************************************")
print(" Setting the two robot to a configuration specified by the controls ")
print("***********************************************************************")
controls = (0.662338, 0.271613, 0.760218, 0.722817, 0.738732, 0.659155, 0.676090, 0.643239, 0.213521, 0.770563, 0.245352, 0.261268, 0.500000, 0.600000)
print("Robot controls: ", controls)
kautham.kMoveRobot(node, controls)
print("\n********************************************************************")
print(" Check collision at config to pick object ")
print("********************************************************************")
controls = (0.587, 0.337, 0.698, 0.698, 0.746, 0.587, 0.44, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5) #configuration to grasp the object
kautham.kIsCollisionFree (node, controls)
print("\n********************************************************")
print(" Setting the control file to just move robot 0 ")
print("********************************************************")
robot_control_file = "controls/right_ur3_with_gripper.cntr"
kautham.kSetRobControlsNoQuery(node, robot_control_file)
print("\n********************************************************************")
print(" Check collision at config to pick object ")
print("********************************************************************")
controls = (0.587, 0.337, 0.698, 0.698, 0.746, 0.587, 0.44) #configuration to grasp the object
kautham.kIsCollisionFree (node, controls)
print("\n********************************************************************")
print(" Moving the robot 0 to a configuration to pick object ")
print("********************************************************************")
controls = (0.587, 0.337, 0.698, 0.698, 0.746, 0.587, 0.44) #configuration to grasp the object
print("Set Robot 0 to configuration specified by controls: ", controls)
kautham.kMoveRobot(node, controls)
print("\n***************************************")
print(" Getting the pose of pawnB1 ")
print("***************************************")
obspos=kautham.kGetObstaclePos(node, "pawnB1")
print("pawnB1 pose=", obspos)
print("\n***************************************************")
print(" Attaching pawnB1 to robot gripper base ")
print("***************************************************")
kautham.kAttachObject(node, "ur3_right", "robotiq_85_base_link", "pawnB1")
print("\n********************************************************************")
print(" Moving the robot 0 to place configuration ")
print("********************************************************************")
controls = (0.782, 0.337, 0.698, 0.698, 0.746, 0.52, 0.44) #configuration to place object
print("Set Robot 0 to configuration specified by controls: ", controls)
kautham.kMoveRobot(node, controls)
print("\n***************************************************")
print(" Dettach pawnB1 ")
print("***************************************************")
kautham.kDetachObject(node, "pawnB1")
print("\n***************************************")
print(" Getting the pose of pawnB1 ")
print("***************************************")
obspos=kautham.kGetObstaclePos(node, "pawnB1")
print("pawnB1 pose=", obspos)
print("\n***************************************")
print(" Getting the pose of robot right ")
print("***************************************")
robotpos=kautham.kGetRobotPos(node, "ur3_right")
print("robot0 pose=", robotpos)
print("\n***************************************")
print(" Getting the pose of robot left ")
print("***************************************")
robotpos=kautham.kGetRobotPos(node, "ur3_left")
print("robot1 pose=", robotpos)
print("\n***************************************")
print(" Moving object pawnB1 ")
print("***************************************")
poseobject = (0.025, 0.025, 0.02, 0.0, 0.0, 1.0, 0.0)#in axis-angle
print("PawnB1 pose = ", poseobject)
kautham.kSetObstaclePos(node,"pawnB1",poseobject)
print("\n***************************************")
print(" Getting the pose of pawnB1 ")
print("***************************************")
obspos=kautham.kGetObstaclePos(node, "pawnB1")
print("pawnB1 pose=", obspos)
print("\n***************************************")
print(" Closing problem ")
print("***************************************")
kautham.kCloseProblem(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()