Skip to content

GitLab

Commit 43e6e618 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

script test_simple_graph uses Python class ConstraintGraph

parent deddea2f
Hide whitespace changes
Inline Side-by-side
script/test_simple_graph.py
View file @ 43e6e618
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.hrp2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp_ros.manipulation import ScenePublisher
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_mathieu'
# Load HRP2 and a screwgun {{{3
robot = Robot ('hrp2')
robot.client.basic.problem.selectPathPlanner ("M-RRT")
robot.loadObjectModel ('screw_gun', 'freeflyer', 'airbus_environment', 'screw_gun', '_nomass', '')
robot.loadObjectModel ('screw_gun', 'freeflyer', 'airbus_environment', 'screw_gun', '_massless', '')
robot.buildCompositeRobot ('hrp2-screw', ['hrp2', 'screw_gun'])
for d in ["hrp2", "screw_gun"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
......@@ -15,9 +17,11 @@ robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
# 3}}}
r = ScenePublisher (robot)
# Define configurations {{{3
half_sitting = robot.getCurrentConfig ();
q1=[0.2561522768052704, 0.009968182216397095, -0.0013343337623661373, 0.9822200021750263, -0.01324065709811156, 0.06488994582367685, 0.1756640181081545, 0.3518980306918394, 0.5587732909530961, -0.00014707196704839167, 0.003348199890377334, 0.2851538622381365, 0.019515985023849783, 0.47568241804625144, 0.034325840512149174, 0.48238375657832383, 0.5252200250587239, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.08183778724042397, -0.002329068502171773, 0.0013672024871911833, -0.03184796508559784, 0.00013468971902929027, -0.006463660186047965, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.0014454298679420635, 0.008692815372600783, -0.17944952816665183, -0.0349066, 0.08186718484328222, -0.0052176502711966674, -0.0014458867527617663, 0.008689383322591662, -0.1789250482962942, -0.0349066, 0.0813567181568653, -0.00521418794593098, -1.0, -0.009968182216397, 0.3399077826256168, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
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]
......@@ -25,29 +29,12 @@ q3=[0.2561522768052704, 0.009968182216397017, -0.0013072533849402136, 0.98223843
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.75, -0.75, 0.75, -0.75, 0.75, -0.75, -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.75, -0.75, 0.75, -0.75, 0.75, -0.75, -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]
# 3}}}
# Generate constraints {{{3
p = ProblemSolver (robot)
p.createGrasp ('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2')
p.createGrasp ('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2')
p.createPreGrasp ('pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
rankcfg = 0; rankvel = 0; lockscrewgun = list ();
for axis in ['x','y','z']:
p.createLockedDofConstraint ('screwgun_lock_' + axis, 'screw_gun/base_joint_xyz', 0, rankcfg, rankvel)
p.createLockedDofConstraint ('screwgun_lock_r' + axis, 'screw_gun/base_joint_SO3', 0, rankcfg + 1, rankvel)
p.isLockedDofParametric ('screwgun_lock_' + axis ,True)
p.isLockedDofParametric ('screwgun_lock_r' + axis ,True)
lockscrewgun.append ('screwgun_lock_' + axis)
lockscrewgun.append ('screwgun_lock_r' + axis)
rankcfg = rankcfg + 1
rankvel = rankvel + 1
graph = ConstraintGraph (robot, 'graph')
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
p.createLockedDofConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , q1[17], 0, 0)
p.createLockedDofConstraint ('lhand_0', 'hrp2/LHAND_JOINT0', q1[18], 0, 0)
p.createLockedDofConstraint ('lhand_1', 'hrp2/LHAND_JOINT1', q1[19], 0, 0)
p.createLockedDofConstraint ('lhand_2', 'hrp2/LHAND_JOINT2', q1[20], 0, 0)
p.createLockedDofConstraint ('lhand_3', 'hrp2/LHAND_JOINT3', q1[21], 0, 0)
p.createLockedDofConstraint ('lhand_4', 'hrp2/LHAND_JOINT4', q1[22], 0, 0)
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
......@@ -58,63 +45,46 @@ for n in jointNames['all']:
jointNames['hrp2'].append (n)
if not n.startswith ("hrp2/LARM"):
jointNames['allButHRP2LeftArm'].append (n)
robot.client.basic.problem.setPassiveDofs ('left-hand-grasp-passive', jointNames['hrp2'])
jointNames ["bottomPart"] = \
['hrp2/base_joint_x',
'hrp2/base_joint_y',
'hrp2/base_joint_z',
'hrp2/base_joint_SO3',
'hrp2/LLEG_JOINT0',
'hrp2/LLEG_JOINT1',
'hrp2/LLEG_JOINT2',
'hrp2/LLEG_JOINT3',
'hrp2/LLEG_JOINT4',
'hrp2/LLEG_JOINT5',
'hrp2/RLEG_JOINT0',
'hrp2/RLEG_JOINT1',
'hrp2/RLEG_JOINT2',
'hrp2/RLEG_JOINT3',
'hrp2/RLEG_JOINT4',
'hrp2/RLEG_JOINT5']
p.createStaticStabilityConstraints ("balance", q1)
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2'])
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
graph = robot.client.manipulation.graph
id = dict()
id["graph" ] = graph.createGraph ('hrp2-screwgun')
id["subgraph"] = graph.createSubGraph ('lefthand')
id["screwgun"] = graph.createNode (id["subgraph"], 'screwgun')
id["free" ] = graph.createNode (id["subgraph"], 'free')
lockscrewgun = p.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock', parametric = True)
id["ungrasp"] = graph.createWaypointEdge (id["screwgun"], id["free"], "ungrasp", 1, False)
id["ungrasp_w"], id["ungrasp_w_node"] = graph.getWaypoint (id["ungrasp"])
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
p.createLockedDofConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , q1[17], 0, 0)
p.createLockedDofConstraint ('lhand_0', 'hrp2/LHAND_JOINT0', q1[18], 0, 0)
p.createLockedDofConstraint ('lhand_1', 'hrp2/LHAND_JOINT1', q1[19], 0, 0)
p.createLockedDofConstraint ('lhand_2', 'hrp2/LHAND_JOINT2', q1[20], 0, 0)
p.createLockedDofConstraint ('lhand_3', 'hrp2/LHAND_JOINT3', q1[21], 0, 0)
p.createLockedDofConstraint ('lhand_4', 'hrp2/LHAND_JOINT4', q1[22], 0, 0)
id[ "grasp"] = graph.createWaypointEdge (id["free"], id["screwgun"], "grasp", 5, True)
id["grasp_w"], id["grasp_w_node"] = graph.getWaypoint (id["grasp"])
p.createStaticStabilityConstraints ("balance", q1)
# 3}}}
# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
id["move_free" ] = graph.createEdge (id["free" ], id["free" ], "move_free" , 1 , False)
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
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.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
graph.setNumericalConstraints (id["screwgun"], ['left-hand-grasp'])
graph.setNumericalConstraintsForPath (id["screwgun"], ['left-hand-grasp-passive'])
graph.setLockedDofConstraints (id["move_free"], 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)
graph.createEdge ('free', 'free', 'move_free', 5)
manip = robot.client.manipulation
graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10)
graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5)
graph.setConstraints (node='screwgun', grasp='l_grasp')
graph.setConstraints (edge='move_free', lockDof = lockscrewgun)
graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)
graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun)
graph.client.graph.setLevelSetConstraints (graph.edges['keep_grasp_ls'], [], lockscrewgun)
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=p.balanceConstraints ())
# 3}}}
p.setInitialConfig (qinit)
p.addGoalConfig (qgoal)
# This projector tends to fail with probability 0.6 (with random configuration)
#manip.graph.statOnConstraint (id["grasp"])
Markdown is supported
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