The Python interface

This section shows the Python interface to the services offered by Kautham.

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 kSetRobControls: Function that wraps the kautham_node/SetRobControls service (see SetRobControls)

  • Function kSetRobControlsNoQuery: Function that wraps the kautham_node/SetRobControlsNoQuery service (see SetRobControlsNoQuery)

  • Function kSetDefaultRobControls: Function that wraps the kautham_node/SetDefaultRobControls service (see SetDefaultRobControls)

  • Function kSetRobotPos: Function that wraps the kautham_node/SetRobotPos service (see SetRobotPos / GetRobpos)

  • Function kGetRobotPos: Function that wraps the kautham_node/GetRobotPos 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 kSetObstaclePos: Function that wraps the kautham_node/SetObstaclePos service (see SetObstaclePos / GetObstaclePos)

  • Function kObstaclePos: Function that wraps the kautham_node/GetObstaclePos service (see SetObstaclePos / GetObstaclePos)

  • Function kAttachObject: Function that wraps the kautham_node/AttachObstacle2RobotLink service (see AttachObstacle2RobotLink)

  • Function kDetachObject: Function that wraps the kautham_node/DetachObstacle service (see DetachObstacle)

Planner functions

  • Function kSetGoal: Function that wraps the kautham_node/SetInit service (see SetInit)

  • Function kSetInit: Function that wraps the kautham_node/SetGoal service (see SetGoal)

  • Function kSetQuery: Function that wraps the kautham_node/SetQuery service (see SetQuery)

  • Function kSetPlannerByName: Function that wraps the kautham_node/SetPlannerByName service (see SetPlannerByName)

  • Function kSetPlannerParameter: Function that wraps the kautham_node/SetPlannerParameter service (see SetPlannerParameter)

  • Function kPathDofNames: Function that wraps the kautham_node/PathDofNames service (see PathDofNames)

  • Function kGetPath: Function that wraps the kautham_node/GetPath service (see GetPath)

  • Function kIsCollisionFree: Function that wraps the kautham_node/CheckCollision service (see CheckCollision)

  • Function kConnect: Function that wraps the kautham_node/Connect service (see Connect)

Solution functions

Visualization function

  • Function kVisualizeScene: Function that wraps the kautham_node/VisualizeScene service (see Visualization services)

Some examples

Planning Example

The following codes loads by default the OMPL_RRTconnect_boxes_world_R2.xml problem and solves it.

It also illustrates the change of the planer parameters, of the queries and of change of planner.

#!/usr/bin/env python3
import rospy
import rospkg
import sys
import os
import random
from std_msgs.msg import String, Time
from geometry_msgs.msg import Pose
rospack =rospkg.RosPack()
import xml.etree.ElementTree as ET
from collections import defaultdict
#Import python module with functions necessary for interfacing with kautham
import kautham_py.kautham_python_interface as kautham

def main():
   #Setting problem files
   ROSpackage_path = rospack.get_path("kautham")
   modelFolder = ROSpackage_path + "/demos/models/"

   #check for arguments
   if len(sys.argv)<2:
       print("Using default boxes_world demo")
       kauthamProblemFile= ROSpackage_path + "/demos/OMPL_geo_demos/boxes_world_R2/OMPL_RRTconnect_boxes_world_R2.xml"
       kauthamproblem = os.path.basename(kauthamProblemFile)
   else:
       kauthamProblemFile= ROSpackage_path + "/" + sys.argv[1]
       kauthamproblem = os.path.basename(sys.argv[1])

   print("Using kautham problem",kauthamProblemFile)
   rospy.loginfo ("Starting Kautham Python Client")
   rospy.init_node("kautham_python_client")

   rospy.loginfo_once(kauthamProblemFile)

   ##Solving the motion planning problem
   #Open kautham problem
   print("***************************************")
   print("   Opening problem                     ")
   print("***************************************")
   kautham.kOpenProblem(modelFolder,kauthamProblemFile)

   #Solve query
   print("***************************************")
   print("   Solving Query                       ")
   print("***************************************")
   path=kautham.kGetPath(1)
   #dofnames=kautham.kPathDofNames()
   #print("dofnames = ", dofnames)

   print("***************************************")
   print("   Solving Query with Range=20"         )
   print("***************************************")
   kautham.kSetPlannerParameter("Range","20")
   kautham.kGetPath(1)

   print("***************************************")
   print("   Solving Random Queries              ")
   print("***************************************")#!/usr/bin/env python3

   for x in range(5):
       init = [random.random(),random.random()]
       goal = [random.random(),random.random()]
       if kautham.kSetQuery(init,goal):
           print("Query valid (init=", init, " goal = ", goal, "). Calling getPath")
           kautham.kGetPath(0) #do not print the path
       else:
           print("Query not valid (init=", init, " goal = ", goal, "). Skipping getPath call")

   print("***************************************")
   print("   Solving Query with an RRT  "         )
   print("***************************************")
   kautham.kSetPlannerByName("omplRRT")
   kautham.kSetPlannerParameter("Range","20")
   kautham.kSetPlannerParameter("Goal Bias","0.05")
   kautham.kSetQuery(ini,goal)
   kautham.kGetPath(1)

   #Close kautham problem
   kautham.kCloseProblem()


if __name__ == '__main__':
   try:
       #Run the main function
       main()
   except rospy.ROSInterruptException:
       pass

Visualization Example

#!/usr/bin/env python3
import rospy
import rospkg
import sys
from std_msgs.msg import String, Time
from geometry_msgs.msg import Pose
rospack =rospkg.RosPack()
import xml.etree.ElementTree as ET
from collections import defaultdict
#Import python module with functions necessary for interfacing with kautham
import kautham_py.kautham_python_interface as kautham


# Global variables
directory=''
Robot_move_control= ''
Robot_pos=[]
taskfile=''
graspedobject= False

#Function to write to xml file in Conf tag
def writePath(taskfile,tex):
  taskfile.write("\t\t<Conf> %s </Conf>\n" % tex)
  return True

def main():
  # Initialise code
  #check for arguments
  if len(sys.argv)<2:
      print("Number of parameters is not correct")
      print (" Should be: $./kautham_client_python_node.py kthconfig.xml")

  print("Using kautham---------------------")

  rospy.loginfo ("Starting Kautham Python Client")
  rospy.init_node("kautham_python_client")

  #Open config file
  ROSpackage_path= rospack.get_path("kautham")
  config_tree = ET.parse(ROSpackage_path+sys.argv[1])
  config_root = config_tree.getroot()
  #Get data from config file
  #Get data for Problem files
  kauthamproblem = config_root.find('Problemfiles').find('kautham').get('name')
  DIRECTORY =config_root.find('Problemfiles').find('directory').get('name')
  print("Using kautham problem",kauthamproblem)

  #Setting problem files
  modelFolder = ROSpackage_path + "/demos/models/"
  global directory
  directory=ROSpackage_path + DIRECTORY#"/demos/OMPL_geo_demos/Table_Rooms_R2/"
  kauthamProblemFile= directory + kauthamproblem

  rvizconfigfile = directory + config_root.find('Problemfiles').find('rviz').get('name')

  rospy.loginfo_once(kauthamProblemFile)

  ##Solving the motion planning problem
  #Open kautham problem
  print("***************************************")
  print("   Opening problem                     ")
  print("***************************************")
  kautham.kOpenProblem(modelFolder,kauthamProblemFile)

  use_joint_state_publisher_gui = False
  kautham.kVisualizeScene(use_joint_state_publisher_gui, rvizconfigfile)

  controls = (0.1,0.2,0.3,0.4,0.5,0.6,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5)
  kautham.kMoveRobot(controls)


  poseobject = (0.025,0.025,0.02,0,0,0,1)
  kautham.kSetObstaclePos("pawnB1",poseobject)


  input("Press Enter to Finalize...")

  #Close kautham problem
  kautham.kCloseProblem()


if __name__ == '__main__':
  try:
      #Run the main function
      main()
  except rospy.ROSInterruptException:
      pass

You can try this visualization option launching the following demos:

$ roslaunch kautham kautham_chess.launch

$ roslaunch kautham kautham_tiago_kitchen.launch