val = v
ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig()
r = placard.rank
q[r:r + 3] = [.4, 0, 1.2]
ps.createStaticStabilityConstraints('balance-romeo',
q,
'romeo',
type=ProblemSolver.FIXED_ON_THE_GROUND)
commonConstraints = Constraints(numConstraints=ps.balanceConstraints(),
lockedJoints=lockHands)
# build graph
rules = [
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", ""], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["", "placard/high"], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
# Loops {{{4
cg.createEdge ('free', 'free', 'move_free', 1)
cg.createEdge ('cup', 'cup', 'cup_keep_grasp', 10)
cg.createEdge ('fridge', 'fridge', 'fridge_keep_grasp', 10)
cg.createEdge ('both', 'both', 'both_keep_grasp', 1)
cg.setConstraints (edge='move_free', lockDof = lockFridge + lockCup)
cg.setConstraints (edge='cup_keep_grasp', lockDof = lockFridge)
cg.setConstraints (edge='fridge_keep_grasp', lockDof = lockCup)
# 4}}}
cg.setConstraints (graph = True, lockDof = lockAll, numConstraints=ps.balanceConstraints ())
# 3}}}
#res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
#if not res[0]:
#raise Exception ('Init configuration could not be projected.')
#
#q_init_proj = res [1]
#res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
#if not res[0]:
#raise Exception ('Goal configuration could not be projected.')
#
#q_goal_proj = res [1]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
cg.client.graph.setTargetNodeList (cg.subGraphId,
val = [v,]
else:
val = v;
ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom ('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig ()
r = placard.rank
q [r:r+3] = [.4, 0, 1.2]
ps.createStaticStabilityConstraints ('balance-romeo', q, 'romeo',
type = ProblemSolver.FIXED_ON_THE_GROUND)
commonConstraints = Constraints (numConstraints = ps.balanceConstraints (),
lockedJoints = lockHands)
# build graph
rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True),
]
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [placard.handles.values ()]
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers,
[placard.name,],
handlesPerObjects,
graph.setNumericalConstraints(id["screwgun"], ['left-hand-grasp'])
graph.setNumericalConstraintsForPath(id["screwgun"],
['left-hand-grasp-passive'])
graph.setLockedDofConstraints(id["move_free"], lockscrewgun)
graph.setLockedDofConstraints(id["pregrasp"], lockscrewgun)
graph.setLockedDofConstraints(id["unpregrasp"], lockscrewgun)
#graph.setNumericalConstraints (id["grasp"], cBalance)
#graph.setNumericalConstraints (id["ungrasp"], cBalance)
#graph.setLockedDofConstraints (id["grasp"], lockscrewgun)
#graph.setLockedDofConstraints (id["ungrasp"], lockscrewgun)
graph.setLockedDofConstraints(id["grasp"],
sum([lockscrewgun, lockbottompart], []))
graph.setLockedDofConstraints(id["ungrasp"],
sum([lockscrewgun, lockbottompart], []))
graph.setLockedDofConstraints(id["keep_align"], lockscrewgun)
graph.setNumericalConstraints(id["graph"], p.balanceConstraints())
graph.setLockedDofConstraints(id["graph"], locklhand)
manip = robot.client.manipulation
#res = manip.problem.applyConstraints ([id["free"]], robot.shootRandomConfig())
#if not res[0]:
#raise StandardError ("Could not project qinit")
#qinit = res[1]
#res = manip.problem.applyConstraints ([id["free"]], robot.shootRandomConfig())
#if not res[0]:
#raise StandardError ("Could not project qgoal")
#qgoal = res[1]
#p.setInitialConfig (qinit)
cg.createEdge('free', 'free', 'move_free', 1)
cg.createEdge('cup', 'cup', 'cup_keep_grasp', 10)
cg.createEdge('fridge', 'fridge', 'fridge_keep_grasp', 10)
cg.createEdge('both', 'both', 'both_keep_grasp', 1)
cg.setConstraints(edge='move_free', lockDof=lockFridge + lockCup)
cg.setConstraints(edge='cup_keep_grasp', lockDof=lockFridge)
cg.setConstraints(edge='fridge_keep_grasp', lockDof=lockCup)
# 4}}}
cg.setConstraints(graph=True,
lockDof=lockAll,
numConstraints=ps.balanceConstraints())
# 3}}}
#res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
#if not res[0]:
#raise Exception ('Init configuration could not be projected.')
#
#q_init_proj = res [1]
#res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
#if not res[0]:
#raise Exception ('Goal configuration could not be projected.')
#
#q_goal_proj = res [1]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
cg.client.graph.setTargetNodeList(cg.subGraphId, [
# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
graph.createEdge ('free', 'free', 'move_free', 5)
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.setLevelSetConstraints ('keep_grasp_ls', lockDof = lockscrewgun)
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ())
# 3}}}
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
r = vf.createRealClient()
pp = PathPlayer (robot.client.basic, r)
# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
graph.createEdge ('free', 'free', 'move_free', 5)
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=ps.balanceConstraints ())
# 3}}}
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client.basic, r)
graph.setNumericalConstraints (id["prescrewgun"], ['left-hand-pregrasp', 'left-hand-pregrasp/ineq_0', 'left-hand-pregrasp/ineq_0.1'])
#graph.setNumericalConstraints (id["prescrewgun"], ['left-hand-pregrasp', 'left-hand-pregrasp/ineq_0'])
graph.setNumericalConstraintsForPath (id["prescrewgun"], ['left-hand-pregrasp-passive'])
graph.setNumericalConstraints (id["screwgun"], ['left-hand-grasp'])
graph.setNumericalConstraintsForPath (id["screwgun"], ['left-hand-grasp-passive'])
graph.setLockedDofConstraints (id["move_free"], lockscrewgun)
graph.setLockedDofConstraints (id["pregrasp"], lockscrewgun)
graph.setLockedDofConstraints (id["unpregrasp"], lockscrewgun)
#graph.setNumericalConstraints (id["grasp"], cBalance)
#graph.setNumericalConstraints (id["ungrasp"], cBalance)
#graph.setLockedDofConstraints (id["grasp"], lockscrewgun)
#graph.setLockedDofConstraints (id["ungrasp"], lockscrewgun)
graph.setLockedDofConstraints (id["grasp"], sum([lockscrewgun, lockbottompart],[]))
graph.setLockedDofConstraints (id["ungrasp"], sum([lockscrewgun, lockbottompart],[]))
graph.setLockedDofConstraints (id["keep_align"], lockscrewgun)
graph.setNumericalConstraints (id["graph"], p.balanceConstraints ())
graph.setLockedDofConstraints (id["graph"], locklhand)
manip = robot.client.manipulation
#res = manip.problem.applyConstraints ([id["free"]], robot.shootRandomConfig())
#if not res[0]:
#raise StandardError ("Could not project qinit")
#qinit = res[1]
#res = manip.problem.applyConstraints ([id["free"]], robot.shootRandomConfig())
#if not res[0]:
#raise StandardError ("Could not project qgoal")
#qgoal = res[1]
#p.setInitialConfig (qinit)
