Exemple #1
0
        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,
Exemple #4
0
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)
Exemple #5
0
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)
Exemple #8
0
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)