Extension - RRT: basic finding example

• Path finding is using sample-based (RRT) methods rather than constrained optimization to find a collision free path

• Path finding is much easier if a final robot pose is given. We here use IK (formulated as KOMO problem) to first compute a final configuration ‘qT’. (Path optimization does this jointly with optimizing the path.)

• Then we can pass the current state and qT to a bi-directional RRT to find a collision free path.

• Note that BotOp (the interface to sim/real) is opened only after we computed the motion. We simply pass the motion to be played by the sim/real.

import robotic as ry
import time

first a minimalistic example for testing:

C = ry.Config()
C.addFrame("base") .setPosition([0,0,.5])

C.addFrame("ego", "base") \
    .setJoint(ry.JT.transXYPhi, [-1.,1.,-1.,1.,-3.,3.]) \
    .setRelativePosition([.2, .0, .0]) \
    .setShape(ry.ST.ssBox, size=[.05, .3, .05, .01]) \
    .setColor([0, 1., 1.]) \
    .setContact(1)

C.addFrame("obstacle") \
    .setPosition([.0, .0, .5]) \
    .setShape(ry.ST.ssBox, size=[.05, .3, .05, .01]) \
    .setColor([1, .5, 0]) \
    .setContact(1)

C.view(False)

q0 = [-.2, 0, 0]
qT = [+.2, 0, 0]

ry.params_clear()
ry.params_add({'rrt/stepsize':.1, 'rrt/verbose': 3}) #this makes it very slow, and displays result, and wait keypress..

rrt = ry.PathFinder()
rrt.setProblem(C, [q0], [qT])
ret = rrt.solve()
print(ret)
path = ret.x

ry.params_print()

del rrt

print('path length:', path.shape)
# display the path
for t in path:
    C.setJointState(t)
    C.view()
    time.sleep(1./path.shape[0])

#this prints all parameters used by the rrt:
ry.params_print()

C = ry.Config()
C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandasTable.g'))
C.view()

C.addFrame('boxR','table') \
    .setRelativePosition([.15,0,.1]) \
    .setShape(ry.ST.ssBox, size=[.1,.1,.1,.02]) \
    .setColor([1,1,0])
C.addFrame('boxL','table') \
    .setRelativePosition([-.15,0,.1]) \
    .setShape(ry.ST.ssBox, size=[.1,.1,.1,.02]) \
    .setColor([1,.5,0])
C.view()

# store the start configuration
q0 = C.getJointState()

# compute a goal configuration
komo = ry.KOMO()
komo.setConfig(C, True)
komo.setTiming(1., 1, 5., 0)
komo.addControlObjective([], 0, 1e-0)
komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq);
komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq);
komo.addObjective([], ry.FS.positionDiff, ['r_gripper', 'boxL'], ry.OT.eq, [1e1]);
komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'boxR'], ry.OT.eq, [1e1]);

ret = ry.NLP_Solver() \
    .setProblem(komo.nlp()) \
    .setOptions( stopTolerance=1e-2, verbose=4 ) \
    .solve()
print(ret)

# that's the goal configuration
qT = komo.getPath()[0]
C.setJointState(qT)
C.view(False, "IK solution")

#define a path finding problem
rrt = ry.PathFinder()
rrt.setProblem(C, [q0], [qT])

ret = rrt.solve()
print(ret)
path = ret.x

# display the path
for t in range(0, path.shape[0]-1):
    C.setJointState(path[t])
    C.view()
    time.sleep(.1)

# run the path with botop
C.setJointState(q0)
ry.params_add({'botsim/verbose': 1., 'physx/motorKp': 10000., 'physx/motorKd': 1000.})
bot = ry.BotOp(C, False)
bot.home(C)

bot.moveAutoTimed(path, 1., 1.)
while bot.getTimeToEnd()>0:
    bot.sync(C, .1)

del bot

del rrt
del C