|
42 | 42 | using namespace barrett;
|
43 | 43 |
|
44 | 44 | void printMenu() {
|
45 |
| - printf("Commands:\n"); |
46 |
| - printf(" j Hold joint positions\n"); |
47 |
| - printf(" p Hold tool position (in Cartesian space)\n"); |
48 |
| - printf(" o Hold tool orientation\n"); |
49 |
| - printf(" b Hold both tool position and orientation\n"); |
50 |
| - printf(" i Idle (release position/orientation constraints)\n"); |
51 |
| - printf(" q Quit\n"); |
| 45 | + printf("Commands:\n"); |
| 46 | + printf(" j Hold joint positions\n"); |
| 47 | + printf(" p Hold tool position (in Cartesian space)\n"); |
| 48 | + printf(" o Hold tool orientation\n"); |
| 49 | + printf(" b Hold both tool position and orientation\n"); |
| 50 | + printf(" i Idle (release position/orientation constraints)\n"); |
| 51 | + printf(" q Quit\n"); |
52 | 52 | }
|
53 | 53 |
|
54 | 54 | template <size_t DOF>
|
55 | 55 | int wam_main(int argc, char **argv, ProductManager &pm,
|
56 | 56 | systems::Wam<DOF> &wam) {
|
57 |
| - wam.gravityCompensate(); |
58 |
| - printMenu(); |
59 |
| - |
60 |
| - std::string line; |
61 |
| - bool going = true; |
62 |
| - while (going) { |
63 |
| - printf(">>> "); |
64 |
| - std::getline(std::cin, line); |
65 |
| - |
66 |
| - switch (line[0]) { |
67 |
| - case 'j': |
68 |
| - printf("Holding joint positions.\n"); |
69 |
| - wam.moveTo(wam.getJointPositions()); |
70 |
| - break; |
71 |
| - |
72 |
| - case 'p': |
73 |
| - printf("Holding tool position.\n"); |
74 |
| - wam.moveTo(wam.getToolPosition()); |
75 |
| - break; |
76 |
| - |
77 |
| - case 'o': |
78 |
| - printf("Holding tool orientation.\n"); |
79 |
| - wam.moveTo(wam.getToolOrientation()); |
80 |
| - break; |
81 |
| - |
82 |
| - case 'b': |
83 |
| - printf("Holding both tool position and orientation.\n"); |
84 |
| - wam.moveTo(wam.getToolPose()); |
85 |
| - break; |
86 |
| - |
87 |
| - case 'i': |
88 |
| - printf("WAM idled.\n"); |
89 |
| - |
90 |
| - // Note that this use of the word "idle" does not mean "Shift-idle". |
91 |
| - // Calling Wam::idle() will disable any of the controllers that may |
92 |
| - // be connected (joint position, tool position, tool orientation, |
93 |
| - // etc.) leaving only gravity compensation. (More specifically, |
94 |
| - // Wam::idle() disconnects any inputs that were connected using |
95 |
| - // Wam::trackReferenceSignal().) |
96 |
| - wam.idle(); |
97 |
| - break; |
98 |
| - |
99 |
| - case 'q': |
100 |
| - case 'x': |
101 |
| - printf("Quitting.\n"); |
102 |
| - wam.moveHome(); |
103 |
| - going = false; |
104 |
| - break; |
105 |
| - |
106 |
| - default: |
107 |
| - if (line.size() != 0) { |
108 |
| - printf("Unrecognized option.\n"); |
109 |
| - printMenu(); |
110 |
| - } |
111 |
| - break; |
112 |
| - } |
113 |
| - } |
114 |
| - |
115 |
| - // Release the WAM if we're holding. This is convenient because it allows |
116 |
| - // users to move the WAM back to some collapsed position before exiting, if |
117 |
| - // they want. |
118 |
| - wam.idle(); |
119 |
| - |
120 |
| - // Wait for the user to press Shift-idle |
121 |
| - pm.getSafetyModule()->waitForMode(SafetyModule::IDLE); |
122 |
| - return 0; |
| 57 | + wam.gravityCompensate(); |
| 58 | + printMenu(); |
| 59 | + |
| 60 | + std::string line; |
| 61 | + bool going = true; |
| 62 | + while (going) { |
| 63 | + printf(">>> "); |
| 64 | + std::getline(std::cin, line); |
| 65 | + |
| 66 | + switch (line[0]) { |
| 67 | + case 'j': |
| 68 | + printf("Holding joint positions.\n"); |
| 69 | + wam.moveTo(wam.getJointPositions()); |
| 70 | + break; |
| 71 | + |
| 72 | + case 'p': |
| 73 | + printf("Holding tool position.\n"); |
| 74 | + wam.moveTo(wam.getToolPosition()); |
| 75 | + break; |
| 76 | + |
| 77 | + case 'o': |
| 78 | + printf("Holding tool orientation.\n"); |
| 79 | + wam.moveTo(wam.getToolOrientation()); |
| 80 | + break; |
| 81 | + |
| 82 | + case 'b': |
| 83 | + printf("Holding both tool position and orientation.\n"); |
| 84 | + wam.moveTo(wam.getToolPose()); |
| 85 | + break; |
| 86 | + |
| 87 | + case 'i': |
| 88 | + printf("WAM idled.\n"); |
| 89 | + |
| 90 | + // Note that this use of the word "idle" does not mean "Shift-idle". |
| 91 | + // Calling Wam::idle() will disable any of the controllers that may |
| 92 | + // be connected (joint position, tool position, tool orientation, |
| 93 | + // etc.) leaving only gravity compensation. (More specifically, |
| 94 | + // Wam::idle() disconnects any inputs that were connected using |
| 95 | + // Wam::trackReferenceSignal().) |
| 96 | + wam.idle(); |
| 97 | + break; |
| 98 | + |
| 99 | + case 'q': |
| 100 | + case 'x': |
| 101 | + printf("Quitting.\n"); |
| 102 | + wam.moveHome(); |
| 103 | + going = false; |
| 104 | + break; |
| 105 | + |
| 106 | + default: |
| 107 | + if (line.size() != 0) { |
| 108 | + printf("Unrecognized option.\n"); |
| 109 | + printMenu(); |
| 110 | + } |
| 111 | + break; |
| 112 | + } |
| 113 | + } |
| 114 | + |
| 115 | + // Release the WAM if we're holding. This is convenient because it allows |
| 116 | + // users to move the WAM back to some collapsed position before exiting, if |
| 117 | + // they want. |
| 118 | + wam.idle(); |
| 119 | + |
| 120 | + // Wait for the user to press Shift-idle |
| 121 | + pm.getSafetyModule()->waitForMode(SafetyModule::IDLE); |
| 122 | + return 0; |
123 | 123 | }
|
0 commit comments