Skip to content
GitLab
Commit 6489b63b authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update test for HRP2 and manipulation planning

parent 890fc317
Hide whitespace changes
Inline Side-by-side
script/test_simple_graph.py
View file @ 6489b63b
......@@ -24,8 +24,8 @@ q1=[0.2561522768052704, 0.009968182216397095, -0.0013343337623661373, 0.98222000
q2=[0.2561522768052704, 0.009968182216397094, -0.0013343337623677983, 0.9822200021750275, -0.013240657098102699, 0.06488994582365894, 0.17566401810815518, 0.3518980306918404, 0.5587732909530987, -0.00014707196704828986, 0.003348199890377807, 0.2851538622381267, 0.019515985023862044, 0.4756824180462511, 0.03432584051214365, 0.48238375657832383, 0.5252200250587228, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.08183778724043055, -0.0023290685021591065, 0.0013672024871913863, -0.03184796508560018, 0.00013468971902932646, -0.006463660186048368, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.001445429867963691, 0.008692815372566085, -0.17944952816666074, -0.0349066, 0.08186718484332876, -0.00521765027117088, -0.0014458867527830789, 0.008689383322556907, -0.17892504829630115, -0.0349066, 0.08135671815690743, -0.005214187945905259, 1.0, -0.009968182216397, 0.3399077826256168, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
q3=[0.2561522768052704, 0.009968182216397017, -0.0013072533849402136, 0.9822384333854748, -0.01312068217341367, 0.06461437559545073, 0.17567154051286343, 0.35191845881842604, 0.5584191613132159, -0.00014417077050741273, 0.003264525687460523, 0.2869014360284614, 0.01968512963006628, 0.47563871278773184, 0.0349066, 0.4823374727974044, 0.5253128675582199, 0.00028141477557467405, -9.224156430856334e-05, 9.57339438119022e-05, 0.00043012512285501805, 9.464640215865539e-05, -0.00027454688131241776, -0.08070554282100409, -0.0022150673918241194, 0.0013507126487957277, -0.031445080182081316, 0.00013303660804169588, -0.0063951082465386055, 0.00018071522532470678, -7.008398345853373e-05, 0.0001432918466161822, 5.0663492959904484e-05, 5.932458587315774e-05, -0.0001501990824631989, -0.0016207277515218107, 0.008331445043308924, -0.1782202190053313, -0.0349066, 0.08120820667180957, -0.004973436857255375, -0.0016211611493169267, 0.008328178972275535, -0.17768239866563243, -0.03490564559358417, 0.08066263806839956, -0.00497014215842449, -0.2561522768052704, -0.009968182216396924, 0.33990778262561817, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
qinit=[6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.9997086753828088, -0.001462864329132029, 0.024091994398753543, 1.4078154478767463e-05, -0.00040620371503693694, 0.06720468781884271, 0.09997150487212819, 0.10619128937732075, -0.030984458364104503, 0.06768151515542697, -9.21491333147205e-05, -0.007395092069129794, 4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975, -0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136, 0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679, 6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079, -0.0011302430393443214, 0.00792613666021656, -0.05431204743596144, -0.020779247681766293, 0.026905347698115048, -0.00494217897984072, -0.0011303560351055939, 0.007923626259361372, -0.05448958126663155, -0.019807309704424767, 0.02612488205235436, -0.004939666182290102, 1.9999999999999933, -0.009968182216395699, 0.3399077826251962, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
#qgoal=
qinit= [6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.9997086753828088, -0.001462864329132029, 0.024091994398753543, 1.4078154478767463e-05, -0.00040620371503693694, 0.06720468781884271, 0.09997150487212819, 0.10619128937732075, -0.030984458364104503, 0.06768151515542697, -9.21491333147205e-05, -0.007395092069129794, 4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975, -0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136, 0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679, 6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079, -0.0011302430393443214, 0.00792613666021656, -0.05431204743596144, -0.020779247681766293, 0.026905347698115048, -0.00494217897984072, -0.0011303560351055939, 0.007923626259361372, -0.05448958126663155, -0.019807309704424767, 0.02612488205235436, -0.004939666182290102, 2, -0.009968182216395699, 0.3399077826251962, 0.7071067811865476, 0, -0.7071067811865475, 0]
qgoal= [6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.9997086753828088, -0.001462864329132029, 0.024091994398753543, 1.4078154478767463e-05, -0.00040620371503693694, 0.06720468781884271, 0.09997150487212819, 0.10619128937732075, -0.030984458364104503, 0.06768151515542697, -9.21491333147205e-05, -0.007395092069129794, 4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975, -0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136, 0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679, 6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079, -0.0011302430393443214, 0.00792613666021656, -0.05431204743596144, -0.020779247681766293, 0.026905347698115048, -0.00494217897984072, -0.0011303560351055939, 0.007923626259361372, -0.05448958126663155, -0.019807309704424767, 0.02612488205235436, -0.004939666182290102, 1, -0.009968182216395699, 0.3399077826251962, 0.7071067811865476, 0, -0.7071067811865475, 0]
p = ProblemSolver (robot)
p.createGrasp ('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2')
......@@ -107,28 +107,34 @@ id["subgraph"] = graph.createSubGraph ('lefthand')
id["screwgun"] = graph.createNode (id["subgraph"], 'screwgun')
id["free" ] = graph.createNode (id["subgraph"], 'free')
id["ungrasp"] = graph.createEdge (id["screwgun"], id["free"], "ungrasp", 1, False)
id[ "grasp"] = graph.createEdge (id["free"], id["screwgun"], "grasp", 10, True)
id["ungrasp"] = graph.createWaypointEdge (id["screwgun"], id["free"], "ungrasp", 1, False)
id["ungrasp_w"], id["ungrasp_w_node"] = graph.getWaypoint (id["ungrasp"])
id["grasp_w"], id["grasp_w_node"] = graph.addWaypoint (id["grasp"], "grasp_w")
id[ "grasp"] = graph.createWaypointEdge (id["free"], id["screwgun"], "grasp", 5, True)
id["grasp_w"], id["grasp_w_node"] = graph.getWaypoint (id["grasp"])
id["move_free" ] = graph.createEdge (id["free" ], id["free" ], "move_free" , 1 , False)
id["keep_grasp"] = graph.createEdge (id["screwgun"], id["screwgun"], "keep_grasp", 10, False)
id["keep_grasp"] = graph.createEdge (id["screwgun"], id["screwgun"], "keep_grasp", 5, False)
id["keep_grasp_ls"] = graph.createLevelSetEdge (id["screwgun"], id["screwgun"], "keep_grasp_ls", 5, False)
graph.setNumericalConstraints (id["screwgun"], ['left-hand-grasp'])
graph.setNumericalConstraintsForPath (id["screwgun"], ['left-hand-grasp-passive'])
graph.setLockedDofConstraints (id["move_free"], lockscrewgun)
graph.setLockedDofConstraints (id["grasp"], lockscrewgun)
graph.setLockedDofConstraints (id["ungrasp"], lockscrewgun)
graph.setNumericalConstraints (id["ungrasp_w_node"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["ungrasp_w"], lockscrewgun)
graph.setLockedDofConstraints (id["grasp"], lockscrewgun)
graph.setNumericalConstraints (id["grasp_w_node"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["grasp_w"], lockscrewgun)
graph.setLevelSetConstraints (id["keep_grasp_ls"], [], lockscrewgun)
graph.setNumericalConstraints (id["graph"], p.balanceConstraints ())
graph.setLockedDofConstraints (id["graph"], locklhand)
manip = robot.client.manipulation
p.setInitialConfig (q1)
p.addGoalConfig (q2)
p.setInitialConfig (qinit)
p.addGoalConfig (qgoal)
# This projector tends to fail with probability 0.6 (with random configuration)
manip.graph.statOnConstraint (id["grasp"])
#manip.graph.statOnConstraint (id["grasp"])
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment