LGP-1: First Mini Interface

Disclaimer: The algorithm interfaced here is absolutely not made or tuned for performance. The purpose of this first interface less to provide the algorithm, but rather to think about: 1. what are interfaces to specify a TAMP problem (see TAMP_Provider, and Logic2KOMO_Translator below), and 2. what are basic computational components that an underlying kinematics/optimization engine should provide, so that a good algorithm could be build on top.

In the current version, the LGP tool needs three things as input: * a configuration, * a TAMP_Provider, which provides a method ‘getNewPlan’, which returns a next possible sequence of logical actions (action=string tuple), * a Logic2KOMO_Translator, which provides a method ‘add_action_constraints’, which adds KOMO constraints for any action.

Eventually, both should be specified by the user in python. For the TAMP_Provider, the user should use plain PDDL and a planner to provide more and more plans. For the Logic2KOMO_Translater, the user can define any translation of logic predicates to geometric constraints - beyond just pick’n’place or push. But for now, for development, I’m using intransparent C++ implementations of these classes to provide defaults.

import robotic as ry
ry.compiled()

problem = 'data/lgp_single_pnp'
#problem = 'data/lgp_bimanual_pnp'

C = ry.Config()
C.addFile(problem+'.g') # change the config geometry (including size of trays) to check if solver still works
#C.view() #or C.watchFile('data/lgp_single_pnp.g') #to edit

trans = ry.default_Logic2KOMO_Translator()
tamp = ry.default_TAMP_Provider(C, problem+'.lgp') # change the logic goal ('terminal') to check is solver still works
lgp = ry.LGP_Tool(C, tamp, trans)

for k in range(10):
    lgp.solve(0)
    plan = lgp.getSolvedPlan()
    print(f'** solution {k}: {plan}', flush=True)
    lgp.view_solved(pause=False) #change to inspect each plan

    for k in range(len(plan)):
        piece = lgp.get_piecewiseMotionProblem(k, True)
        ret = ry.NLP_Solver(piece.nlp(), verbose=0 ) .solve()
        piece.set_viewer(C.get_viewer()) # to prevent too many windows popping up
        piece.view_play(False, f'PIECE solution {k} {plan[k]}')

    path = lgp.get_fullMotionProblem(True)
    ret = ry.NLP_Solver(path.nlp(), verbose=0 ) .solve()
    path.set_viewer(C.get_viewer()) # to prevent too many windows popping up
    path.view_play(False, f'FULL PATH solution {plan}')

lgp.view_close()

del C