factory.setGrippers(grippers)
factory.setObjects([
placard.name,
], handlesPerObjects, [
[],
])
factory.setRules(rules)
factory.generate()
cg.addConstraints(graph=True, constraints=commonConstraints)
cg.initialize()
with (open("./level-set-edge.log", "w")) as f:
for e, id in cg.edges.items():
if e.find("_ls") != -1:
f.write("edge {}\n{}\n".format(e, cg.displayEdgeConstraints(e)))
# Define initial and final configurations
q_goal = [
-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841,
-0.08000440760954532, 0.06905332841243099, -0.09070086400314036,
0.9902546570793265, 0.2097693637044623, 0.19739743868699455,
-0.6079135018296973, 0.8508704420155889, -0.39897628829947995,
-0.05274298289004072, 0.20772797293264825, 0.1846394290733244,
-0.49824886682709824, 0.5042013065348324, -0.16158420369261683,
-0.039828502509861335, -0.3827070014985058, -0.24118425356319423,
1.0157846623463191, 0.5637424355124602, -1.3378817283780955,
-1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06,
1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06,
0.35936687035487364, -0.32595302056157444, -0.33115291290191723,
0.20387672048126043, 0.9007626913161502, -0.39038645767349395,
