| 25 | | |
| 26 | | == Tutorial 2: Using the 3D simulator == |
| 27 | | |
| 28 | | ''Files for this tutorial are available in [https://framagit.org/fma38/Py4bot/tree/master/py4bot/examples/tutorial_2 py4bot/examples/tutorial_2/]'' |
| 29 | | |
| 30 | | Here, we will see how to use the 3D simulation stuff. |
| 31 | | |
| 32 | | As before, we create a new empty dir in our home dir: |
| 33 | | |
| 34 | | {{{ |
| 35 | | $ mkdir tutorial_2 |
| 36 | | $ cd tutorial_2 |
| 37 | | }}} |
| 38 | | |
| 39 | | {{{ |
| 40 | | #!python |
| 41 | | |
| 42 | | #!/usr/bin/env python |
| 43 | | # -*- coding: utf-8 -*- |
| 44 | | |
| 45 | | import math |
| 46 | | |
| 47 | | import visual |
| 48 | | |
| 49 | | from py4bot.api import * |
| 50 | | |
| 51 | | import settings |
| 52 | | }}} |
| 53 | | |
| 54 | | Nothing fancy, here. |
| 55 | | |
| 56 | | {{{ |
| 57 | | #!python |
| 58 | | |
| 59 | | class Hexapod3D(Robot3D): |
| 60 | | def _createBody(self): |
| 61 | | return Body(settings.LEGS_ORIGIN) |
| 62 | | |
| 63 | | def _createLegs(self): |
| 64 | | legs = {} |
| 65 | | legIk = {} |
| 66 | | for legIndex in settings.LEGS_INDEX: |
| 67 | | legs[legIndex] = Leg3Dof(legIndex, {'coxa': Coxa(), 'femur': Femur(), 'tibia': Tibia()}, settings.FEET_NEUTRAL[legIndex]) |
| 68 | | legIk[legIndex] = Leg3DofIk(settings.LEGS_GEOMETRY[legIndex]) |
| 69 | | |
| 70 | | return legs, legIk |
| 71 | | }}} |
| 72 | | |
| 73 | | Same as before, except that we inherits from {{{Robot3D}}}, instead of {{{Robot}}}. |
| 74 | | |
| 75 | | {{{ |
| 76 | | #!python |
| 77 | | |
| 78 | | def _createActuatorPool(self): |
| 79 | | driver = Driver3D() |
| 80 | | pool = ActuatorPool(driver) |
| 81 | | |
| 82 | | num = 0 |
| 83 | | for leg in self._legs.values(): |
| 84 | | |
| 85 | | # Create joints actuators |
| 86 | | coxa3D = Coxa3D(leg.coxa, num, settings.LEGS_GEOMETRY[leg.index]['coxa']) |
| 87 | | pool.add(coxa3D) |
| 88 | | num += 1 |
| 89 | | |
| 90 | | femur3D = Femur3D(leg.femur, num, settings.LEGS_GEOMETRY[leg.index]['femur']) |
| 91 | | pool.add(femur3D) |
| 92 | | num += 1 |
| 93 | | |
| 94 | | tibia3D = Tibia3D(leg.tibia, num, settings.LEGS_GEOMETRY[leg.index]['tibia']) |
| 95 | | pool.add(tibia3D) |
| 96 | | num += 1 |
| 97 | | |
| 98 | | # Init 3D view |
| 99 | | coxa3D.frame = self._body3D |
| 100 | | femur3D.frame = coxa3D |
| 101 | | tibia3D.frame = femur3D |
| 102 | | |
| 103 | | coxa3D.pos = (settings.LEGS_ORIGIN[leg.index]['x'], 0, -settings.LEGS_ORIGIN[leg.index]['y']) |
| 104 | | femur3D.pos = (settings.LEGS_GEOMETRY[leg.index]['coxa'], 0, 0) |
| 105 | | tibia3D.pos = (settings.LEGS_GEOMETRY[leg.index]['femur'], 0, 0) |
| 106 | | |
| 107 | | coxa3D.rotate(angle=math.radians(settings.LEGS_ORIGIN[leg.index]['gamma0']), axis=(0, 1, 0)) |
| 108 | | |
| 109 | | return pool |
| 110 | | }}} |
| 111 | | |
| 112 | | Here, we use specific 3D actuators: {{{Coxa3D}}}, {{{Femur3D}}}, {{{Tibia3D}}}; they all use the [http://vpython.org VPython] library to represent themselves on the screen. |
| 113 | | |
| 114 | | Note that we don't use a custom actuator mapping; nums are automatically incremented. |
| 115 | | |
| 116 | | We also need to configure the hierarchy, and the relative positions of the actuators. |
| 117 | | |
| 118 | | ''Note: this may change in the future; I'm working on a simpler way to do all this''. |
| 119 | | |
| 120 | | {{{ |
| 121 | | #!python |
| 122 | | |
| 123 | | class Gamepad(RemoteControl): |
| 124 | | def _createFrontend(self): |
| 125 | | return Thrustmaster(settings.THRUSTMASTER_PATH) |
| 126 | | |
| 127 | | def _buildComponents(self): |
| 128 | | self._addConfig("walk") |
| 129 | | self._addConfig("body") |
| 130 | | |
| 131 | | self._addComponent(Button, command=self.selectNextConfig, key="button_008", trigger="hold") |
| 132 | | self._addComponent(Button, command=self.selectPreviousConfig, key="button_009", trigger="hold") |
| 133 | | |
| 134 | | self._addComponent(Button, command=self.robot.incBodyPosition, key="button_004", mapper=MapperSetValue(dz=+5)) |
| 135 | | self._addComponent(Button, command=self.robot.incBodyPosition, key="button_005", mapper=MapperSetValue(dz=-5)) |
| 136 | | self._addComponent(Joystick, configs="walk", command=GaitSequencer().walk, keys=("analog_02", "analog_03", "analog_00"), mapper=MapperWalk()) |
| 137 | | |
| 138 | | self._addComponent(Analog, configs="body", command=self.robot.setBodyExtraPosition, key="analog_00", mapper=MapperSetMultiply('yaw', coef=-15)) |
| 139 | | self._addComponent(Analog, configs="body", command=self.robot.setBodyExtraPosition, key="analog_03", mapper=MapperSetMultiply('pitch', coef=-15)) |
| 140 | | self._addComponent(Analog, configs="body", command=self.robot.setBodyExtraPosition, key="analog_02", mapper=MapperSetMultiply('roll', coef=15)) |
| 141 | | }}} |
| 142 | | |
| 143 | | Same as before... |
| 144 | | |
| 145 | | {{{ |
| 146 | | #!python |
| 147 | | |
| 148 | | def main(): |
| 149 | | def addGait(gaitClass, gaitName): |
| 150 | | group = settings.GAIT_LEGS_GROUPS[gaitName] |
| 151 | | params = settings.GAIT_PARAMS['default'] |
| 152 | | for key1, value1 in settings.GAIT_PARAMS[gaitName].items(): |
| 153 | | for key2, value2 in value1.items(): |
| 154 | | params[key1][key2] = value2 |
| 155 | | gait = gaitClass(gaitName, group, params) |
| 156 | | GaitManager().add(gait) |
| 157 | | |
| 158 | | scene = visual.display(width=settings.SCENE_WIDTH, height=settings.SCENE_HEIGHT) |
| 159 | | ground = visual.box(axis=(0, 1, 0), pos=(0, 0, 0), length=0.1, height=500, width=500, color=visual.color.gray(0.75), opacity=0.5) |
| 160 | | |
| 161 | | addGait(GaitTripod, "tripod") |
| 162 | | addGait(GaitTetrapod, "tetrapod") |
| 163 | | addGait(GaitRiple, "riple") |
| 164 | | addGait(GaitWave, "metachronal") |
| 165 | | addGait(GaitWave, "wave") |
| 166 | | GaitManager().select("riple") |
| 167 | | |
| 168 | | robot = Hexapod3D() |
| 169 | | |
| 170 | | remote = Gamepad(robot) |
| 171 | | |
| 172 | | GaitSequencer().start() |
| 173 | | remote.start() |
| 174 | | |
| 175 | | robot.setBodyPosition(z=30) |
| 176 | | |
| 177 | | robot.mainLoop() |
| 178 | | |
| 179 | | remote.stop() |
| 180 | | remote.join() |
| 181 | | GaitSequencer().stop() |
| 182 | | GaitSequencer().join() |
| 183 | | |
| 184 | | if __name__ == "__main__": |
| 185 | | main() |
| 186 | | }}} |
| 187 | | |
| 188 | | The only difference, here, is the scene size init, and the ground addition (optional). |
| 189 | | |
| 190 | | === Settings === |
| 191 | | |
| 192 | | {{{ |
| 193 | | #!python |
| 194 | | |
| 195 | | # -*- coding: utf-8 -*- |
| 196 | | |
| 197 | | from py4bot.common import config |
| 198 | | |
| 199 | | |
| 200 | | # Legs index |
| 201 | | LEGS_INDEX = ('RF', 'RM', 'RR', 'LR', 'LM', 'LF') |
| 202 | | |
| 203 | | # Legs geometry |
| 204 | | LEGS_GEOMETRY = { |
| 205 | | 'RM': {'coxa': 25, 'femur': 45, 'tibia': 65}, |
| 206 | | 'RF': {'coxa': 25, 'femur': 45, 'tibia': 65}, |
| 207 | | 'LF': {'coxa': 25, 'femur': 45, 'tibia': 65}, |
| 208 | | 'LM': {'coxa': 25, 'femur': 45, 'tibia': 65}, |
| 209 | | 'LR': {'coxa': 25, 'femur': 45, 'tibia': 65}, |
| 210 | | 'RR': {'coxa': 25, 'femur': 45, 'tibia': 65} |
| 211 | | } |
| 212 | | |
| 213 | | # Legs origin |
| 214 | | LEGS_ORIGIN = { |
| 215 | | 'RM': {'x': 35., 'y': 0., 'gamma0' : 0.}, |
| 216 | | 'RF': {'x': 35., 'y': 65., 'gamma0' : 30.}, |
| 217 | | 'LF': {'x': -35., 'y': 65., 'gamma0' : 150.}, |
| 218 | | 'LM': {'x': -35., 'y': 0., 'gamma0' : 180.}, |
| 219 | | 'LR': {'x': -35., 'y': -65., 'gamma0' : 210.}, |
| 220 | | 'RR': {'x': 35., 'y': -65., 'gamma0' : 330.}, |
| 221 | | } |
| 222 | | |
| 223 | | # Legs feet neutral position |
| 224 | | FEET_NEUTRAL = { |
| 225 | | 'RM': LEGS_GEOMETRY['RM']['coxa'] + LEGS_GEOMETRY['RM']['femur'], |
| 226 | | 'RF': LEGS_GEOMETRY['RF']['coxa'] + LEGS_GEOMETRY['RF']['femur'], |
| 227 | | 'LF': LEGS_GEOMETRY['LF']['coxa'] + LEGS_GEOMETRY['LF']['femur'], |
| 228 | | 'LM': LEGS_GEOMETRY['LM']['coxa'] + LEGS_GEOMETRY['LM']['femur'], |
| 229 | | 'LR': LEGS_GEOMETRY['LR']['coxa'] + LEGS_GEOMETRY['LR']['femur'], |
| 230 | | 'RR': LEGS_GEOMETRY['RR']['coxa'] + LEGS_GEOMETRY['RR']['femur'], |
| 231 | | } |
| 232 | | |
| 233 | | # Gaits |
| 234 | | GAIT_LEGS_GROUPS = { |
| 235 | | 'tripod': (('RM', 'LF', 'LR'), ('RF', 'LM', 'RR')), |
| 236 | | 'tetrapod': (('RR', 'LM'), ('RF', 'LR'), ('RM', 'LF')), |
| 237 | | 'riple': (('RR',), ('LM',), ('RF',), ('LR',), ('RM',), ('LF',)), |
| 238 | | 'metachronal': (('RR',), ('LM',), ('RF',), ('LR',), ('RM',), ('LF',)), |
| 239 | | 'wave': (('RR',), ('RM',), ('RF',), ('LR',), ('LM',), ('LF',)) |
| 240 | | } |
| 241 | | |
| 242 | | # Gaits parameters |
| 243 | | GAIT_PARAMS = { |
| 244 | | 'default': { |
| 245 | | 'length': { |
| 246 | | 'min': 5., |
| 247 | | 'max': 40. |
| 248 | | }, |
| 249 | | 'angle': { |
| 250 | | 'min': 0.5, |
| 251 | | 'max': 5. |
| 252 | | }, |
| 253 | | 'height': { |
| 254 | | 'min': 20., |
| 255 | | 'max': 40. |
| 256 | | }, |
| 257 | | 'speed': { |
| 258 | | 'min': 25., |
| 259 | | 'max': 250. |
| 260 | | } |
| 261 | | }, |
| 262 | | 'tripod': {}, |
| 263 | | 'tetrapod': {}, |
| 264 | | 'riple': {}, |
| 265 | | 'metachronal': {}, |
| 266 | | 'wave': {} |
| 267 | | } |
| 268 | | |
| 269 | | # Gamepad path |
| 270 | | THRUSTMASTER_PATH = "/dev/input/by-id/usb-Mega_World_Thrustmaster_dual_analog_3.2-event-joystick" |
| 271 | | |
| 272 | | # VPython scene default size |
| 273 | | SCENE_WIDTH = 640 |
| 274 | | SCENE_HEIGHT = 480 |
| 275 | | |
| 276 | | }}} |
| 277 | | |
| 278 | | We removed the servos mapping/calibration, as we don't need them anymore, and we just added the scene default size. |