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(5):
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
[ ]: