grasp_ball.py 3.8 KB
Newer Older
Florent Lamiraux's avatar
Florent Lamiraux committed
1
from math import sqrt
2
from hpp import Transform
3
from hpp.corbaserver.manipulation import ConstraintGraph, Constraints
Florent Lamiraux's avatar
Florent Lamiraux committed
4
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
27
#  Warning the order of the nodes is important. When checking in which node
Florent Lamiraux's avatar
Florent Lamiraux committed
28
29
#  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
40
ps.createTransformationConstraint ('placement', '', ballName,
                                   [0,0,0.025,0, 0, 0, 1],
                                   [False, False, True, True, True, False,])
41
#  Create complement constraint
42
43
44
ps.createTransformationConstraint ('placement/complement', '', ballName,
                                   [0,0,0.025,0, 0, 0, 1],
                                   [True, True, False, False, False, True,])
45
46
47
48
49

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

## Set constraints of nodes and edges
50
51
52
53
54
55
56
57
graph.setConstraints (node='placement', constraints = \
                      Constraints (numConstraints = ['placement'],))
graph.setConstraints (node='grasp',
                      constraints = Constraints (numConstraints = ['grasp']))
graph.setConstraints (edge='transit', constraints = \
                      Constraints (numConstraints = ['placement/complement']))
graph.setConstraints (edge='grasp-ball', constraints = \
                      Constraints (numConstraints = ['placement/complement']))
Florent Lamiraux's avatar
Florent Lamiraux committed
58
# These edges are in node 'grasp'
59
60
graph.setConstraints (edge='transfer',     constraints = Constraints ())
graph.setConstraints (edge='release-ball', constraints = Constraints ())
61
62
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
63
graph.initialize ()
64
65

## Project initial configuration on state 'placement'
66
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
67
68
69
70
q2 = q1 [::]
q2 [7] = .2

## Project goal configuration on state 'placement'
71
res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
72
73
74
75
76

## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

77
pp = PathPlayer (r, ps.client.basic)
78
79
80
81
82
83
84
85
86

## 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
87
88
89
90
  gripperPose = Transform (robot.getJointPosition (gripperName))
  ballPose = Transform (robot.getJointPosition (ballName))
  gripperGraspsBall = gripperPose.inverse () * ballPose
  gripperAboveBall = Transform (gripperGraspsBall)
91
  gripperAboveBall.translation [2] += .1