UPennDev2/steps at master · UPenn-RoboCup/UPennDev2 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
Step 1:
Door handle (the stem of the handle, not tip) is x=0.41, y=0.32 meters away from the chest

armManipulation (#2)
armDoor (#3) - weights = {1,1,0} is important


Left Arm
armTeleop: go from (0.38, 0.3, 0) to (0.46, .3, 0)

armTeleop: go from (0.46, 0.3, 0) to (0.46, .3, 0.07)

armTeleop: go from (0.46, .3, 0.07) to (0.5, .3, 0.09)

armTeleop: go from (0.5, .3, 0.09) to (0.7, .3, 0.07)

armTeleop: go from (0.7, .3, 0.07) to (0.7, .3, 0.0)

Right arm now!
armTeleop: go from (armManipulation) to (0.55 -0.20 -0.1)

Back to the left arm
armTeleop: go from (0.7, .3, 0.07) to (0.4, .3, 0.0)

Again with the right
armTeleop: go from (0.55 -0.20 -0.1) to (0.62 -0.13 -0.14)
NOTE: This is difficult

Now waist:
Got o 30degrees