manipulation.py 3.57 KB
Newer Older
Florent Lamiraux's avatar
Florent Lamiraux committed
1
from math import sqrt
2
from hpp import Transform
Florent Lamiraux's avatar
Florent Lamiraux committed
3
4
from hpp.corbaserver.manipulation import ConstraintGraph
from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
5
6
7

vf.loadEnvironmentModel (Ground, 'ground')
vf.loadObjectModel (Pokeball, 'pokeball')
8
robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,2.,
9
10
                                              -1.0001, 1.0001,-1.0001, 1.0001,
                                              -1.0001, 1.0001,-1.0001, 1.0001,])
11
12
13

r = vf.createViewer ()

14
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
15
16
17
18
19
20
21
r (q1)

## Create constraint graph
graph = ConstraintGraph (robot, 'graph')

## Create constraint of relative position of the ball in the gripper when ball
## is grasped
22
ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
Florent Lamiraux's avatar
Florent Lamiraux committed
23
24
ps.createTransformationConstraint ('grasp', gripperName, ballName,
                                   ballInGripper, 6*[True,])
25
26

## Create nodes and edges
Florent Lamiraux's avatar
Florent Lamiraux committed
27
28
29
#  Warning the order of the nodes may be important. When checking in which node
#  a configuration lies, node constraints will be checked in the order of node
#  creation.
30
31
32
33
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')
Florent Lamiraux's avatar
Florent Lamiraux committed
34
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
35
36
37

## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
38
39

ps.createTransformationConstraint ('placement', '', ballName, [0,0,0.025,0, 0, 0, 1], [False, False, True, True, True, False,])
40
#  Create complement constraint
41
ps.createTransformationConstraint ('placement/complement', '', ballName, [0,0,0.025,0, 0, 0, 1], [True, True, False, False, False, True,])
42
43
44
45
46
47
48
49

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',
Florent Lamiraux's avatar
Florent Lamiraux committed
50
                      numConstraints = ['placement/complement'])
51
graph.setConstraints (edge='grasp-ball',
Florent Lamiraux's avatar
Florent Lamiraux committed
52
53
54
55
                      numConstraints = ['placement/complement'])
# These edges are in node 'grasp'
graph.setConstraints (edge='transfer',     numConstraints = [])
graph.setConstraints (edge='release-ball', numConstraints = [])
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
                     (graph.nodes ['placement'], q1)
q2 = q1 [::]
q2 [7] = .2

## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
                     (graph.nodes ['placement'], q2)

## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
Florent Lamiraux's avatar
Florent Lamiraux committed
71
ps.selectPathProjector ("Progressive", 0.1)
72
73
74
75
76
77
78
79
80
81
82

pp = PathPlayer (ps.client.basic, r)

## Build relative position of the ball with respect to the gripper
for i in range (100):
  q = robot.shootRandomConfig ()
  res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
  if res and robot.isConfigValid (q3): break;

if res:
  robot.setCurrentConfig (q3)
Florent Lamiraux's avatar
Florent Lamiraux committed
83
84
85
86
  gripperPose = Transform (robot.getJointPosition (gripperName))
  ballPose = Transform (robot.getJointPosition (ballName))
  gripperGraspsBall = gripperPose.inverse () * ballPose
  gripperAboveBall = Transform (gripperGraspsBall)
87
  gripperAboveBall.translation [2] += .1