quick note; fixing bug when running klampt collision-free motion planning demo code

wanted a quick collision-free motion planning demo, found klampt, went with it since there were actually working examples

ran into some errors when trying to run klamp documentation code

here are my fixes, and to replicate i include the installation instructions i used

install

http://motion.cs.illinois.edu/software/klampt/latest/pyklampt_docs/Manual-Installation.html#

pip install klampt pyopengl pyqt5 pyqtgraph pillow cvxpy
git clone https://github.com/krishauser/Klampt-examples
cd Klampt-examples/Python3/demos
python kbdrive.py ../../data/tx90roll.xml

I briefly installed from source and decided if I had to go down that route I wasn’t interested enough

install examples

git clone https://github.com/krishauser/Klampt-examples
cd Klampt-examples/Python3/demos
python kbdrive.py ../../data/tx90roll.xml

output

Python 3.10.5 (v3.10.5:f377153967, Jun  6 2022, 12:36:10) [Clang 13.0.0 (clang-1300.0.29.30)] on darwin
Type "help", "copyright", "credits" or "license" for more information.
>>> import klampt
>>> klampt.__version__
'0.9.0'

fixed code

in same folder, tried to run

cd Klampt-examples/Python3/demos
python teacup.py

the fixed code

import klampt
from klampt.plan import cspace,robotplanning
from klampt.model import trajectory
from klampt.io import resource
import time

from klampt.plan.robotcspace import RobotCSpace
from klampt.model import collide

world = klampt.WorldModel()
#world.readFile("Klampt-examples/data/tx90cuptable.xml")
success = world.readFile("../../data/tx90cuptable.xml")
if not success:
    raise RuntimeError("Unable to load world")

robot = world.robot(0)

#this is the CSpace that will be used.  Standard collision and joint limit constraints
#will be checked
#space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05)
space = RobotCSpace(robot,collide.WorldCollider(world))

#fire up a visual editor to get some start and goal configurations
qstart = robot.getConfig()
qgoal = robot.getConfig()
save,qstart = resource.edit("Start config",qstart,"Config",world=world)
#it's worthwile to make sure that it's feasible
while save and not space.feasible(qstart):
    print("Start configuration isn't feasible, please pick one that is collision-free")
    save,qstart = resource.edit("Start config",qstart,"Config",world=world)

save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)
while save and not space.feasible(qgoal):
    print("Goal configuration isn't feasible, please pick one that is collision-free")
    save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)

#  --------------

settings = {'type':'rrt',
    'perturbationRadius':0.5,
    'bidirectional':True,
    'shortcut':True,
    'restart':True,
    'restartTermCond':"{foundSolution:1,maxIters:1000}"
}
t0 = time.time()
print("Creating planner...")
#Manual construction of planner
planner = cspace.MotionPlan(space, **settings)
planner.setEndpoints(qstart,qgoal)

print("Planner creation time",time.time()-t0)
t0 = time.time()
print("Planning...")
numIters = 0
for round in range(10):
    planner.planMore(500)
    numIters += 1
    if planner.getPath() is not None:
      break

print('\t*' * 6)
print("Planning time,",numIters,"iterations",time.time()-t0)
print('\t*' * 6)

path = planner.getPath()
if path is not None:
    print("Got a path with",len(path),"milestones")
else:
    print("No feasible path was found")



#  --------------

#provide some debugging information
V,E = planner.getRoadmap()
print(len(V),"feasible milestones sampled,",len(E),"edges connected")

print("CSpace stats:")
spacestats = space.getStats()
for k in sorted(spacestats.keys()):
    print(" ",k,":",spacestats[k])

print("Planner stats:")
planstats = planner.getStats()
for k in sorted(planstats.keys()):
    print(" ",k,":",planstats[k])

if path:
    #save planned milestone path to disk
    print("Saving to my_plan.configs")
    resource.set("my_plan.configs",path,"Configs")

    #visualize path as a Trajectory resource
    traj = trajectory.RobotTrajectory(robot,range(len(path)),path)
    resource.edit("Planned trajectory",traj,world=world)

    #Here's another way to do it: visualize path in the vis module
    from klampt import vis
    vis.add("world",world)
    vis.animate(("world",robot.getName()),path)
    vis.add("trajectory",traj)   #this draws the end effector trajectory
    vis.spin(float('inf'))

explanation

error 1. RuntimeError: Invalid robot index

The error here is actually, the path is incorrect for the world file.

#world.readFile("Klampt-examples/data/tx90cuptable.xml")
world.readFile("../../data/tx90cuptable.xml")

error 2. ValueError invalid length

The interactive window to choose a pose does pop up, but after hit “okay” immediate crashes

ValueError: Invalid length of embedded vector space vector: 12 should be 7

after digging around some other demo/test files, I believe the issues is the robot has 7 joints and the girpper has 5 joints

and there are two ways to specify a planner, one is like a higherlevel “robotplanning” and the other uses lower level “cspace” modules, and on the beginning of the page the example defines the space using robotplanning, but later define the planner using cspace.MotionPlanning.

this causes some issue i assume with loading the world consistently re: 7 vs 12 dimensions

anyway, use robotcspace

#space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05)

space = RobotCSpace(robot,collide.WorldCollider(world))

proof

after manually picking the following points

qstart =[0.0, 0.0, 0.0, -1.3000000000000007, 0.0, 0.0, 0.0, 0.0, 0.048014007722364006, 0.048014007722364006, -0.048014007722364006, 0.048014007722364006]

qgoal = [0.0, 0.0, 0.39999999999999997, -2.18, 0.18, 0.9199999999999999, 0.0, 0.0, 0.048014007722364006, 0.048014007722364006, -0.048014007722364006, 0.048014007722364006]

then waiting 2 minutes for the motion planner (actually)

i get the following video

notes

more example code at: https://gitq.com/krishauser/Klampt

IK would be something like this

from klampt.model import ik

print(robot.numLinks())
obj = ik.objective(robot.link(robot.numLinks()-1),local=[0,0,0],world=[0.5,0,0.5])

you can also run

python gl_vis_widgets.py ../../data/tx90cuptable.xml , manually move arm,
and use menu item “print config” and the coordinates will print out on the terminal

orange narwhals