Tilt Table

This sample project includes a round rimmed table driven with roll and pitch actuators. The world includes a ball rolling around the table. This was developed as an exploration of hybrid control: a human operator can steer the tray using mouse gestures while collecting data, then switch to an automatic mode to drive the table using a model regressed from the gesture data.

It is also a technology demonstration for the following Webots features:

The regression is performed using the Support Vector Machine module within the scikit-learn library.

../_images/tilt-table.png

Screenshot of Webots model of the tilt-table robot. The camera view appears in the upper left corner, with the object location marked by a white box. The blue object above the table marks the camera location. The white cone marks the SpotLight location.

Tilt Table World File

This model is demonstrated in the tilt-table.wbt world available within the Webots.zip archive.

A few notes:

  • The Camera node is a subclass of Solid and so has a children list which can hold the Shapes used to represent the camera itself.

  • The Camera has several slots for special feature nodes. This model uses the Recognition nodes to identify objects. Note that red ball has the recognitionColors field set for this to work.

  • The SpotLight node is a pure light source; in this model it appears within a Transform which also includes a Shape with Cone so that the location is visible.

  • The SpotLight is not directly controllable, but is wrapped in a controllable LED node which can set its color.

  • The Mesh node uses a relative path in the url field to specify an STL file; this keeps the world file portable across machines.

  1#VRML_SIM R2022a utf8
  2WorldInfo {
  3  basicTimeStep 5
  4}
  5Viewpoint {
  6  orientation -0.3052720823884429 -0.02625041344364109 0.9519032889470767 3.225206713187698
  7  position 0.7998569010554496 0.06830636341970958 1.7040116542364134
  8}
  9Background {
 10  skyColor [
 11    0.1 0.1 0.1
 12  ]
 13}
 14DirectionalLight {
 15  direction -0.4 -0.5 -1
 16  castShadows TRUE
 17}
 18RectangleArena {
 19  floorSize 2 2
 20}
 21pedestal {
 22  width 0.5
 23  depth 0.5
 24  height 1
 25}
 26Robot {
 27  translation 0 0 1
 28  children [
 29    HingeJoint {
 30      jointParameters HingeJointParameters {
 31        anchor 0 0 0.1
 32      }
 33      device [
 34        RotationalMotor {
 35          name "j1"
 36        }
 37      ]
 38      endPoint Solid {
 39        translation 0 0 0.1
 40        rotation 1 0 0 0
 41        children [
 42          DEF disc1 Transform {
 43            rotation 0 1 0 1.5708
 44            children [
 45              Shape {
 46                appearance GlossyPaint {
 47                  baseColor 0.772549 0.2 0.192157
 48                }
 49                geometry Cylinder {
 50                  height 0.01
 51                  radius 0.02
 52                }
 53              }
 54            ]
 55          }
 56          HingeJoint {
 57            jointParameters HingeJointParameters {
 58              axis 0 1 0
 59              anchor 0 0 -0.1
 60            }
 61            device [
 62              RotationalMotor {
 63                name "j2"
 64              }
 65            ]
 66            endPoint Solid {
 67              translation 0 0 0.020000000000000004
 68              rotation 0 1 0 0
 69              children [
 70                DEF table Transform {
 71                  rotation 1 0 0 1.5708
 72                  children [
 73                    Shape {
 74                      appearance DEF tableColor GlossyPaint {
 75                        baseColor 0.192157 0.772549 0.589319
 76                      }
 77                      geometry Mesh {
 78                        url [
 79                          "../stl/tilt-tray.stl"
 80                        ]
 81                      }
 82                    }
 83                  ]
 84                }
 85              ]
 86              boundingObject USE table
 87              physics Physics {
 88                density -1
 89                mass 2
 90                centerOfMass [
 91                  0 0.05 0
 92                ]
 93                inertiaMatrix [
 94                  0.03 0.03 0.06
 95                  0 0 0
 96                ]
 97              }
 98            }
 99          }
100        ]
101        physics Physics {
102          density -1
103          mass 2
104          centerOfMass [
105            0 0.05 0
106          ]
107          inertiaMatrix [
108            0.03 0.03 0.06
109            0 0 0
110          ]
111        }
112      }
113    }
114    DEF base Transform {
115      translation 0 0 0.025
116      children [
117        Shape {
118          appearance GlossyPaint {
119            baseColor 0.180392 0 1
120          }
121          geometry Cylinder {
122            height 0.05
123            radius 0.2
124          }
125        }
126      ]
127    }
128    DEF cameraTransform Transform {
129      translation 0 0 1
130      rotation 0 1 0 1.5708
131      children [
132        Camera {
133          rotation 1 0 0 3.14159
134          children [
135            Transform {
136              rotation 0 1 0 -1.5707953071795862
137              children [
138                Shape {
139                  appearance DEF cameraColor GlossyPaint {
140                    baseColor 0.0631418 0.0591135 0.308217
141                  }
142                  geometry Cone {
143                    bottomRadius 0.03
144                    height 0.1
145                  }
146                }
147                Shape {
148                  appearance USE cameraColor
149                  geometry Cylinder {
150                    height 0.1
151                    radius 0.02
152                  }
153                }
154              ]
155            }
156          ]
157          fieldOfView 0.6
158          recognition Recognition {
159            occlusion FALSE
160            frameColor 1 1 1
161          }
162        }
163      ]
164    }
165    DEF spotLight Transform {
166      translation -0.0515544 0.409762 1.07217
167      rotation -0.11680496430032018 -0.8872287288318889 0.4462978635957732 -1.4962553071795863
168      children [
169        Transform {
170          rotation 0 1 0 -1.5707953071795862
171          children [
172            Shape {
173              appearance GlossyPaint {
174                baseColor 0.266102 0.266102 0.266102
175              }
176              geometry Cone {
177                bottomRadius 0.05
178                height 0.1
179              }
180            }
181          ]
182        }
183        LED {
184          children [
185            SpotLight {
186              attenuation 0 0 1
187              beamWidth 0.7
188              color 0 0 0
189              direction 1 0 0
190              intensity 3
191              on FALSE
192              castShadows TRUE
193            }
194          ]
195          name "spotlight"
196          color []
197          gradual TRUE
198        }
199      ]
200    }
201  ]
202  name "tilt-table"
203  controller "tilt_table"
204}
205Solid {
206  translation 0 0 1.2
207  children [
208    DEF ball Shape {
209      appearance GlossyPaint {
210        baseColor 1 0 0.180591
211      }
212      geometry Sphere {
213        radius 0.05
214      }
215    }
216  ]
217  name "ball"
218  boundingObject USE ball
219  physics Physics {
220  }
221  recognitionColors [
222    1 0 0
223  ]
224}

Tilt Table Control Code

  1# tilt_table.py
  2#
  3# Sample Webots controller file for driving the
  4# two-DOF tilt table device.
  5#
  6# No copyright, 2022, Garth Zeglin.  This file is
  7# explicitly placed in the public domain.
  8
  9# References:
 10#   https://scikit-learn.org/stable/modules/svm.html
 11#   https://www.cyberbotics.com/doc/reference/led?tab-language=python
 12#   https://www.cyberbotics.com/doc/reference/camera?tab-language=python
 13
 14print("loading tilt_table.py...")
 15
 16# Import the Webots simulator API.
 17from controller import Robot
 18from controller import Mouse
 19from controller import Keyboard
 20
 21# Import standard Python libraries.
 22import math, random
 23
 24# Import scikit libraries.
 25from sklearn import svm
 26import numpy as np
 27
 28print("tilt_table.py waking up...")
 29print("Use the mouse to control the table tilt, then press A to toggle automatic mode.")
 30
 31# Define the time step in milliseconds between
 32# controller updates.
 33EVENT_LOOP_DT = 20
 34
 35# Request a proxy object representing the robot to
 36# control.
 37robot = Robot()
 38
 39# Fetch handle for the 'base' joint motor.
 40motor1 = robot.getDevice('j1')
 41motor2 = robot.getDevice('j2')
 42
 43# Enable computer keyboard input for user control.
 44keyboard = Keyboard()
 45keyboard.enable(EVENT_LOOP_DT)
 46last_key = None
 47
 48# Request mouse updates for user input.
 49mouse = Mouse()
 50mouse.enable(EVENT_LOOP_DT)
 51
 52# Enable the overhead camera.
 53camera = robot.getDevice('camera')
 54camera.enable(EVENT_LOOP_DT)
 55camera.recognitionEnable(EVENT_LOOP_DT)
 56
 57# Connect to the spotlight.
 58spotlight = robot.getDevice('spotlight')
 59
 60################################################################
 61# Global buffer to record machine states for policy regression.
 62# The system observes tuples [ball_u, ball_v, j1, j2]
 63# The policy regression maps [ball_u, ball_v] -> [d_j1, d_j2]
 64samples = []
 65J1_policy = None
 66J2_policy = None
 67automatic = False
 68
 69def make_policy(history):
 70    # return a pair of models J1 and J2 which map the current state to the next actuator command
 71    J1 = svm.SVR()
 72    J2 = svm.SVR()
 73    ndata = np.array(history)
 74    states = ndata[:,0:2]  # slice representing current state
 75    Y1 = ndata[:,2]        # slice representing J1 policy output
 76    Y2 = ndata[:,3]        # slice representing J2 policy output
 77    J1.fit(states, Y1)     # fit the J1 model
 78    J2.fit(states, Y2)     # fit the J2 model
 79    return J1, J2
 80
 81################################################################
 82# Run an event loop until the simulation quits,
 83# indicated by the step function returning -1.
 84while robot.step(EVENT_LOOP_DT) != -1:
 85
 86    # Read simulator clock time in seconds (discretized by EVENT_LOOP_DT milliseconds).
 87    t = robot.getTime()
 88
 89    # Read the camera ball tracker state.
 90    objects = camera.getRecognitionObjects()
 91    if objects is None or len(objects) == 0:
 92        print("Warning: no ball detected.")
 93        ball = None
 94    else:
 95        # read the [u, v] integer pixel location of ball
 96        # [0,0] is upper left, [64,64] is lower right
 97        ball = objects[0].get_position_on_image()
 98
 99    # Drive the light based on ball position.
100    if ball is not None:
101        if abs(ball[0]-32) < 10 and abs(ball[1]-32) < 10:
102            spotlight.set(0xffffff) # 0xRRGGBB color integer
103        else:
104            spotlight.set(0x000000)
105
106    # Read any computer keyboard keypresses.  Returns -1 or an integer keycode while a key is held down.
107    # This is debounced to detect only changes.
108    key = keyboard.getKey()
109    if key != last_key:
110        last_key = key        
111        if key != -1:
112            # convert the integer key number to a lowercase single-character string
113            letter = chr(key).lower()
114            # Set mode based on keypresses.
115            if letter == 'a':
116                if automatic is False:
117                    print("Entering automatic mode with %d input samples." % (len(samples)))
118                    automatic = True
119                    J1_policy, J2_policy = make_policy(samples)
120                else:
121                    print("Exiting automatic mode.")
122                    automatic = False
123                    samples = []
124
125    if automatic:
126        # in automatic mode, use the policy to select the next action
127        if ball is not None:
128            # scale the samples to unity range; SVM is sensitive to scaling
129            state = [0.01*ball[0], 0.01*ball[1]]
130            angle1 = 0.1 * J1_policy.predict([state])[0]
131            angle2 = 0.1 * J2_policy.predict([state])[0]
132            angle1 = min(max(angle1, -0.1), 0.1)
133            angle2 = min(max(angle2, -0.1), 0.1)
134            motor1.setPosition(angle1)
135            motor2.setPosition(angle2)
136            # print("Policy:", state, angle1, angle2)
137
138    else:
139        # Read the mouse state to control the robot.
140        # The (u,v) coordinates are (0,0) in the upper left and (1,1) in the lower right.
141        ms = mouse.getState()
142        if (not math.isnan(ms.u)) and (not math.isnan(ms.v)):
143            angle1 = -0.2 * (ms.u - 0.5)
144            angle2 =  0.2 * (ms.v - 0.5)
145            motor1.setPosition(angle1)
146            motor2.setPosition(angle2)
147
148            # record the policy trajectory
149            if ball is not None:
150                # scale the samples to unity range; SVM is sensitive to scaling
151                samp = [0.01*ball[0], 0.01*ball[1], 10*angle1, 10*angle2]
152                samples.append(samp)
153                # print("Sample: ", samp)