Esempi in Python per setConstantRightHandSide

Esempio n. 1

graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')

## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', '', ballName,
                                   [0,0,0.025,0, 0, 0, 1],
                                   [False, False, True, True, True, False,])
#  Create complement constraint
ps.createTransformationConstraint ('placement/complement', '', ballName,
                                   [0,0,0.025,0, 0, 0, 1],
                                   [True, True, False, False, False, True,])

ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('placement/complement', False)

## Set constraints of nodes and edges
graph.addConstraints (node='placement', constraints = \
                      Constraints (numConstraints = ['placement'],))
graph.addConstraints (node='grasp',
                      constraints = Constraints (numConstraints = ['grasp']))
graph.addConstraints (edge='transit', constraints = \
                      Constraints (numConstraints = ['placement/complement']))
graph.addConstraints (edge='grasp-ball', constraints = \
                      Constraints (numConstraints = ['placement/complement']))
# These edges are in node 'grasp'
graph.addConstraints (edge='transfer',     constraints = Constraints ())
graph.addConstraints (edge='release-ball', constraints = Constraints ())
ps.selectPathValidation ("Dichotomy", 0)

Esempio n. 2

File: grasp_ball_in_box.py Progetto: jossdeloste/ISAE-SUPAERO

                                   [True, True, False, False, False, True,])
# gripper above ball
ps.createTransformationConstraint ('gripper-above-ball', gripperName, ballName,
                                   [0,0.137,0,0.5, 0.5, -0.5, 0.5],				
                                   [True, True, True, True, True, True,])
# ball above the ground i.e in the gripper
ps.createTransformationConstraint ('ball-above-ground', '', ballName,
                                   [0,0,0.15,0, 0, 0, 1],				
                                   [False, False, True, True, True, False,])
# translation accross z while gripper is above the box
ps.createTransformationConstraint ('Box', '', gripperName,
								   [0,0,0.15,0,0,1,0],
								   [True, True, False, True, True, True])

# Setting up of constraints
ps.setConstantRightHandSide('placement', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('gripper-above-ball', True)
ps.setConstantRightHandSide('ball-above-ground', True)
ps.setConstantRightHandSide('Box',False)

               
#Define Edges constraints                          
graph.setConstraints (edge='transit', numConstraints = ['placement/complement'])
graph.setConstraints (edge='approach-ball',numConstraints = ['placement/complement'])
graph.setConstraints (edge='move-gripper-away', numConstraints = ['placement/complement'])
graph.setConstraints (edge='grasp-ball', numConstraints = ['Box'])
graph.setConstraints (edge='move-gripper-up', numConstraints = ['placement/complement'])
graph.setConstraints (edge='transfer',     numConstraints = [])
graph.setConstraints (edge='approach-ground', numConstraints = [])
graph.setConstraints (edge='take-ball-away', numConstraints = [])

Esempio n. 3

File: manipulation.py Progetto: florent-lamiraux/florent-lamiraux.github.io

graph.createGrasp ('grasp', 'ur5/gripper', 'pokeball/handle')

## Create nodes and edges
graph.createNode (['grasp', 'placement'])
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'placement')

## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [False, False, True, True, True, False,])
#  Create complement constraint
ps.createTransformationConstraint ('placement/complement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [True, True, False, False, False, True,])

ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('placement/complement', False)

## Set constraints of nodes and edges
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (edge='transit',
                      numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = ['grasp'])
graph.setConstraints (edge='grasp-ball',
                      numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='release-ball',
                      numConstraints = ['placement', 'placement/complement'])

## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \

Esempio n. 4

File: grasp_ball.py Progetto: humanoid-path-planner/hpp-practicals

ps.createTransformationConstraint(
    "placement/complement",
    "",
    ballName,
    [0, 0, 0.025, 0, 0, 0, 1],
    [
        True,
        True,
        False,
        False,
        False,
        True,
    ],
)

ps.setConstantRightHandSide("placement", True)
ps.setConstantRightHandSide("placement/complement", False)

# Set constraints of nodes and edges
graph.addConstraints(
    node="placement",
    constraints=Constraints(
        numConstraints=["placement"],
    ),
)
graph.addConstraints(node="grasp", constraints=Constraints(numConstraints=["grasp"]))
graph.addConstraints(
    edge="transit", constraints=Constraints(numConstraints=["placement/complement"])
)
graph.addConstraints(
    edge="grasp-ball", constraints=Constraints(numConstraints=["placement/complement"])

Esempio n. 5

                                      True,
                                      False,
                                  ])

#  Create complement constraint
ps.createTransformationConstraint('placement/complement', '', ballName,
                                  [0, 0, 0.025, 0, 0, 0, 1], [
                                      True,
                                      True,
                                      False,
                                      False,
                                      False,
                                      True,
                                  ])

ps.setConstantRightHandSide('placementBallOnGround', True)
ps.setConstantRightHandSide('placementBallAboveGround', True)

ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('placementAboveBall', True)

## Set constraints of nodes and edges
graph.addConstraints(node='placement',
                     constraints=Constraints(
                         numConstraints=['placementBallOnGround'], ))
graph.addConstraints(
    node='gripper-above-ball',
    constraints=Constraints(
        numConstraints=['placementBallOnGround', 'placementAboveBall']))
graph.addConstraints(
    node='grasp-placement',

Esempio n. 6

graph.createEdge ('grasp', 'ball-above-ground', 'approach-ground', 1, 'grasp')

## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', '', ballName, [0,0,0.025,0, 0, 0, 1], [False, False, True, True, True, False,])

#  Create complement constraint
ps.createTransformationConstraint ('placement/complement', '', ballName, [0,0,0.025,0, 0, 0, 1], [True, True, False, False, False, True,])

ps.createTransformationConstraint ('above-ball', gripperName, ballName, [0, 0.237, 0, 0.5, 0.5, -0.5, 0.5], [True, True, True, True, True, True,])

ps.createTransformationConstraint ('above-ground', '', ballName, [0,0,0.2,0, 0, 0, 1], [False, False, True, True, True, False,])

ps.createTransformationConstraint('z-only', '', gripperName, [0,0,0.2,0,0,1], [True, True, False, True, True, True])

ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('above-ball', True)
ps.setConstantRightHandSide ('above-ground', True)
ps.setConstantRightHandSide ('placement/complement', False)
ps.setConstantRightHandSide ('z-only', False)

## Set constraints of nodes and edges
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='gripper-above-ball', numConstraints = ['placement','above-ball'])
graph.setConstraints (node='grasp-placement', numConstraints = ['grasp', 'placement'])
graph.setConstraints (node='ball-above-ground', numConstraints = ['grasp', 'above-ground'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])

graph.setConstraints (edge='transit', numConstraints = ['placement/complement'])
graph.setConstraints (edge='approach-ball', numConstraints = ['placement/complement'])
graph.setConstraints (edge='move-gripper-up', numConstraints = ['placement/complement'])

Esempio n. 7

                                  ballInGripper, 6 * [
                                      True,
                                  ])

ps.createTransformationConstraint('gripper-above-ball', gripperName, ballName,
                                  [0, .3, 0, 0.5, 0.5, -0.5, 0.5], 6 * [
                                      True,
                                  ])
ps.createTransformationConstraint('ball-above-ground', '', ballName,
                                  [0, 0, 0.3, 0, 0, 0, 1],
                                  [False, False, True, True, True, False])
ps.createTransformationConstraint('change-in-altitude-only', '', gripperName,
                                  [0, 0, 0.3, 0, 0, 0, 1],
                                  [True, True, False, True, True, True])

ps.setConstantRightHandSide('placement', True)
ps.setConstantRightHandSide('gripper-above-ball', True)
ps.setConstantRightHandSide('ball-above-ground', True)
ps.setConstantRightHandSide('placement/complement', False)
ps.setConstantRightHandSide('change-in-altitude-only', False)

## Set constraints of nodes and edges
graph.setConstraints(node='placement', numConstraints=['placement'])
graph.setConstraints(node='grasp', numConstraints=['grasp'])
graph.setConstraints(node='ball-above-ground',
                     numConstraints=['grasp', 'ball-above-ground'])
graph.setConstraints(node='grasp-placement',
                     numConstraints=['grasp', 'placement'])
graph.setConstraints(node='gripper-above-ball',
                     numConstraints=['gripper-above-ball', 'placement'])