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

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].getPositionOnImage()
 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)