test_graph_two_hands.py 8.15 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
# vim: foldmethod=marker foldlevel=2
2
3
4
from hpp.corbaserver.manipulation.pr2 import Robot
from math import sqrt

Joseph Mirabel's avatar
Joseph Mirabel committed
5
# Load robot and object. {{{3
6
7
8
9
robot = Robot ('pr2', rootJointType = "anchor")
robot.client.manipulation.robot.setRootJointPosition ('pr2', (-3.2, -4, 0, 1, 0, 0, 0))
robot.loadObjectModel ('box', 'freeflyer', 'hpp_tutorial', 'box', '', '')
robot.buildCompositeRobot ('pr2-box', ['pr2', 'box'])
10
robot.client.manipulation.robot.loadEnvironmentModel ("iai_maps", "kitchen_area", '', '', "")
11
robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1])
Joseph Mirabel's avatar
Joseph Mirabel committed
12
# 3}}}
13
14
15
16

from hpp_ros.manipulation import ScenePublisher
r = ScenePublisher (robot)

Joseph Mirabel's avatar
Joseph Mirabel committed
17
# Define configurations. {{{3
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
q_goal = q_init [::]

rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_init [rank:rank+3] = [-2.5, -3.6, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
c = sqrt (2) / 2
q_init [rank:rank+4] = [c, 0, c, 0]
r (q_init)

rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [c, 0, -c, 0]
r (q_goal)
del c

q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836]
Joseph Mirabel's avatar
Joseph Mirabel committed
46
# 3}}}
47
48
49
50
51
52
53
54
55

robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)

from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver (robot)

Joseph Mirabel's avatar
Joseph Mirabel committed
56
# Create constraints. {{{3
57
58
59
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')

60
61
robot.client.manipulation.problem.createPlacementConstraint (
  'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top')
62

63
64
65
66
67
68
69
70
71
72
73
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("pr2"):
    jointNames['pr2'].append (n)
  if not n.startswith ("pr2/l_gripper"):
    jointNames['allButPR2LeftArm'].append (n)

cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2'])
74

75
cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', jointNames['pr2'])
76

77
78
cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')
79

80
lockbox = p.lockFreeFlyerJoint ('box/base_joint', 'box_lock', parametric = True)
81
82
83
84
85
86
87
88
89
90
91

locklhand = ['l_l_finger','l_r_finger'];
p.createLockedDofConstraint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', 0.5, 0, 0)
p.createLockedDofConstraint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5, 0, 0)

lockrhand = ['r_l_finger','r_r_finger'];
p.createLockedDofConstraint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', 0.5, 0, 0)
p.createLockedDofConstraint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5, 0, 0)

lockhands = ['l_l_finger','l_r_finger','r_l_finger','r_r_finger'];

Joseph Mirabel's avatar
Joseph Mirabel committed
92
# 3}}}
93

94
# Create the graph. {{{3
95

96
cg.createNode (['both', 'right', 'left', 'free'])
97

98
cg.setConstraints (node='free', numConstraints=['box_placement'])
99
100
101
# This is not need as the object is locked in node free.
#graph.setNumericalConstraintsForPath (id["free"], ['box_placement'])

Joseph Mirabel's avatar
Joseph Mirabel committed
102
# Right hand {{{4
103
cg.setConstraints (node='right', grasp='r_grasp')
104

105
cg.createWaypointEdge ('right', 'free', 'r_ungrasp', nb=1, weight=1)
106

107
cg.createWaypointEdge ('free', 'right', 'r_grasp', nb=1, weight=10)
108

109
110
111
112
113
114
115
#graph.setLockedDofConstraints (id["r_ungrasp_w"], lockbox)
cg.setConstraints (edge='r_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='r_ungrasp_n0', pregrasp = 'r_pregrasp', numConstraints = ['box_placement'])
cg.setConstraints (edge='r_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='r_grasp_n0', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='r_grasp_e0', lockDof = lockbox)
Joseph Mirabel's avatar
Joseph Mirabel committed
116
# 4}}}
117

Joseph Mirabel's avatar
Joseph Mirabel committed
118
# Left hand {{{4
119
cg.setConstraints (node = 'left', grasp = 'l_grasp')
120

121
cg.createWaypointEdge ('left', 'free', 'l_ungrasp', 1, 1)
122

123
cg.createWaypointEdge ('free', 'left', 'l_grasp', 1, 10)
124

125
126
127
128
129
130
cg.setConstraints (edge='l_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='l_ungrasp_n0', pregrasp = 'l_pregrasp', numConstraints = ['box_placement'])
cg.setConstraints (edge='l_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='l_grasp_n0', pregrasp = 'l_pregrasp')
cg.setConstraints (edge='l_grasp_e0', lockDof = lockbox)
Joseph Mirabel's avatar
Joseph Mirabel committed
131
# 4}}}
132

Joseph Mirabel's avatar
Joseph Mirabel committed
133
# Both hands {{{4
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
cg.setConstraints (node='both', grasp = ['l_grasp', 'r_grasp'])

cg.createWaypointEdge ('both', 'right', 'b_l_ungrasp', 1, 1)

cg.createWaypointEdge ('right', 'both', 'b_l_grasp', 1, 10)

cg.setConstraints (edge='b_l_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_l_ungrasp_n0', grasp = 'r_grasp', pregrasp = 'l_pregrasp')
cg.setConstraints (edge='b_l_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='b_l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_l_grasp_n0', grasp = 'r_grasp', pregrasp = 'l_pregrasp')
cg.setConstraints (edge='b_l_grasp_e0', lockDof = lockbox)

cg.createWaypointEdge ('both', 'left', 'b_r_ungrasp', 1, 1)

cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10)

cg.setConstraints (edge='b_r_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_r_ungrasp_n0', grasp = 'l_grasp', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='b_r_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='b_r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_r_grasp_n0', grasp = 'l_grasp', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='b_r_grasp_e0', lockDof = lockbox)
Joseph Mirabel's avatar
Joseph Mirabel committed
157
# 4}}}
158

Joseph Mirabel's avatar
Joseph Mirabel committed
159
# Loops {{{4
160
cg.createEdge ('free', 'free', 'move_free', 1)
161

162
163
cg.createEdge ('left', 'left', 'l_keep_grasp', 5)
cg.createLevelSetEdge ('left', 'left', 'l_keep_grasp_ls', 10)
164

165
166
cg.createEdge ('right', 'right', 'r_keep_grasp', 5)
cg.createLevelSetEdge ('right', 'right', 'r_keep_grasp_ls', 10)
167

168
169
170
cg.setConstraints (edge='move_free', lockDof = lockbox)
cg.client.graph.setLevelSetConstraints (cg.edges['l_keep_grasp_ls'], [], lockbox)
cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox)
Joseph Mirabel's avatar
Joseph Mirabel committed
171
# 4}}}
172

173
174
cg.setConstraints (graph = True, lockDof = lockhands)
# 3}}}
175

176
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
177
178
if not res[0]:
  raise Exception ('Init configuration could not be projected.')
179
180
q_init_proj = res [1]
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
181
182
if not res[0]:
  raise Exception ('Goal configuration could not be projected.')
183
184
185
q_goal_proj = res [1]
p.setInitialConfig (q_init_proj)
p.addGoalConfig (q_goal_proj)