|
1 | 1 | # Import Gepetto viewer helpwer class
|
2 |
| -from hpp.gepetto import Viewer |
3 |
| -# Import Problem solver (holds most of the generated data) |
4 |
| -from hpp.corbaserver.problem_solver import ProblemSolver |
5 | 2 | # Import robot. Needed to create a robot instance for the viewer application
|
6 | 3 | from hpp.corbaserver.affordance import Robot
|
| 4 | +# Import the affordance helper class to extract useful surface |
| 5 | +# objects from the environment, and create an instance of affordanceTool |
| 6 | +from hpp.corbaserver.affordance.affordance import AffordanceTool |
| 7 | +# Import Problem solver (holds most of the generated data) |
| 8 | +from hpp.corbaserver.problem_solver import ProblemSolver |
| 9 | +from hpp.gepetto import Viewer |
7 | 10 |
|
8 | 11 | # Create instance of the hyq robot, the problem solver and the viewer
|
9 | 12 | robot = Robot('hyq')
|
10 |
| -ps = ProblemSolver (robot) |
11 |
| -r = Viewer (ps) |
| 13 | +ps = ProblemSolver(robot) |
| 14 | +r = Viewer(ps) |
12 | 15 |
|
13 | 16 | # Setting initial configuration to show on the viewer
|
14 |
| -q_init = robot.getCurrentConfig () |
15 |
| -q_init [0:3] = [-2, 0, 0.63] |
16 |
| -robot.setCurrentConfig (q_init) |
17 |
| -r (q_init) |
| 17 | +q_init = robot.getCurrentConfig() |
| 18 | +q_init[0:3] = [-2, 0, 0.63] |
| 19 | +robot.setCurrentConfig(q_init) |
| 20 | +r(q_init) |
18 | 21 |
|
19 | 22 | # Set coulour variables for different affordance types
|
20 | 23 | SupportColour = [0.0, 0.95, 0.80]
|
21 | 24 | LeanColour = [0.9, 0.5, 0]
|
22 | 25 |
|
23 |
| -# Import the affordance helper class to extract useful surface |
24 |
| -# objects from the environment, and create an instance of affordanceTool |
25 |
| -from hpp.corbaserver.affordance.affordance import AffordanceTool |
26 |
| -afftool = AffordanceTool () |
| 26 | +afftool = AffordanceTool() |
27 | 27 |
|
28 | 28 | afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05])
|
29 | 29 | afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05])
|
30 | 30 |
|
31 | 31 | # Load obstacle models and visualise affordances. When loading an obstacle,
|
32 | 32 | # the affordance analysis is done automatically.
|
33 |
| -afftool.loadObstacleModel ("hpp-affordance-corba", "darpa", "planning", r) |
34 |
| -afftool.loadObstacleModel ("hpp-affordance-corba", "box", "box1", r) |
| 33 | +afftool.loadObstacleModel("hpp-affordance-corba", "darpa", "planning", r) |
| 34 | +afftool.loadObstacleModel("hpp-affordance-corba", "box", "box1", r) |
35 | 35 | afftool.visualiseAffordances('Support', r, SupportColour)
|
36 | 36 |
|
37 | 37 | # If an object is translated or rotated, the affordance analysis must
|
38 | 38 | # be relaunched.
|
39 | 39 | # First, delete the already created affordance objects (this function also
|
40 | 40 | # deletes them from viewer)
|
41 |
| -afftool.deleteAffordances(r,'box1/base_link_0') |
| 41 | +afftool.deleteAffordances(r, 'box1/base_link_0') |
42 | 42 | # Next, move the obstacle and recompute its position (updates viewer)
|
43 |
| -ps.moveObstacle('box1/base_link_0', [0,1,0.25, 1,0,0,0]) |
| 43 | +ps.moveObstacle('box1/base_link_0', [0, 1, 0.25, 1, 0, 0, 0]) |
44 | 44 | r.computeObjectPosition()
|
45 | 45 | # Now, reanalyse the object that was moved
|
46 | 46 | afftool.analyseObject('box1/base_link_0')
|
|
53 | 53 |
|
54 | 54 | # Some Lean affordances are no more valid due to the orientation of
|
55 | 55 | # the obstacle
|
56 |
| -afftool.loadObstacleModel ("hpp-affordance-corba", "box", "box2", r) |
57 |
| -afftool.deleteAffordances(r,'box2/base_link_0') |
58 |
| -ps.moveObstacle('box2/base_link_0', [0,1.5,0.5, 0.985,0.174,0,0]) |
| 56 | +afftool.loadObstacleModel("hpp-affordance-corba", "box", "box2", r) |
| 57 | +afftool.deleteAffordances(r, 'box2/base_link_0') |
| 58 | +ps.moveObstacle('box2/base_link_0', [0, 1.5, 0.5, 0.985, 0.174, 0, 0]) |
59 | 59 | r.computeObjectPosition()
|
60 | 60 | afftool.analyseObject('box2/base_link_0')
|
61 | 61 | afftool.visualiseAffordances('Support', r, SupportColour, 'box2/base_link_0')
|
62 | 62 | afftool.visualiseAffordances('Lean', r, LeanColour, 'box2/base_link_0')
|
63 | 63 |
|
64 | 64 | # No support affordances are found due to the orientation of the obstacle
|
65 |
| -afftool.loadObstacleModel ("hpp-affordance-corba", "box", "box3", r) |
66 |
| -afftool.deleteAffordances(r,'box3/base_link_0') |
67 |
| -ps.moveObstacle('box3/base_link_0', [0,2,0.75, 0.940,0.342,0,0]) |
| 65 | +afftool.loadObstacleModel("hpp-affordance-corba", "box", "box3", r) |
| 66 | +afftool.deleteAffordances(r, 'box3/base_link_0') |
| 67 | +ps.moveObstacle('box3/base_link_0', [0, 2, 0.75, 0.940, 0.342, 0, 0]) |
68 | 68 | r.computeObjectPosition()
|
69 | 69 | afftool.analyseObject('box3/base_link_0')
|
70 | 70 | afftool.visualiseAffordances('Support', r, SupportColour, 'box3/base_link_0')
|
71 | 71 | afftool.visualiseAffordances('Lean', r, LeanColour, 'box3/base_link_0')
|
72 |
| - |
0 commit comments