Conveyor

This sample project includes a robot which can manipulate a large object balanced on top using three driven wheels. This model demonstrates several key concepts: parallel-drive, holonomic kinematics, and simulator friction and collision modeling.

This model is demonstrated in the conveyor-demo.wbt world.

../_images/conveyor-demo1.png

Screenshot of Webots model of the conveyor robot with three driven wheels making frictional contact with the semi-transparent manipulandum balanced on top.

Sample Robot Control Code

  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
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
# conveyor.py
#
# Sample Webots controller file for driving the three-wheel
# conveyor demo.  The robot comprises three driven wheels
# arranged in a triangle.  A large passive object (the
# 'manipulandum') is balanced on top of the wheels for them to
# move.
#
# No copyright, 2021, Garth Zeglin.  This file is
# explicitly placed in the public domain.

print("loading conveyor.py...")

# Import the Webots simulator API.
from controller import Robot

# Import standard Python modules
import math, random

print("conveyor.py waking up...")

# Define the time step in milliseconds between
# controller updates.
EVENT_LOOP_DT = 20

# Request a proxy object representing the robot to
# control.
robot = Robot()

# Fetch handles for the joint sensors.
sensors = (robot.getDevice('joint1'),
           robot.getDevice('joint2'),
           robot.getDevice('joint3'))

# Specify the sampling rate for the joint sensors.
for s in sensors:
    s.enable(EVENT_LOOP_DT)

# Fetch handle for each conveyor wheel motor.  The motor1 axis
# is positioned along X, so rotations will move the object along
# Y.  Motor2 is 120 degrees CCW from motor1.
motors = (robot.getDevice('motor1'),
          robot.getDevice('motor2'),
          robot.getDevice('motor3'))

# Define a few constant which match the model to be used for
# kinematics calculations.  All units are meters to match the
# simulation.
base_radius  = 0.3
wheel_radius = 0.05

wheel_origins = ((0.3, 0.0, 0.1),
                 (-0.15,0.259808, 0.1),
                 (-0.15,-0.259808,0.1))

wheel_axes    = ((1.0,0.0,0.0),
                 (-0.5,0.866025,0),
                 (-0.5,-0.866025,0))

# The wheel tangent axes define the direction of travel of the
# top contact point for a positive wheel rotation.
wheel_tangents = ((0.0,-1.0,0.0),
                  (0.866025, 0.5,0),
                  (-0.866025, 0.5,0))

# In this example the motor will use velocity control,
# configured by setting an infinite position target and then
# using setVelocity() to issue motor commands.
for motor in motors:
    motor.setPosition(math.inf)
    motor.setVelocity(0)

################################################################
# Dot product utility function.  It would be better to use
# numpy, but this avoids adding the dependency.
def dot_product(v1, v2):
    accum = 0.0
    for e1, e2 in zip(v1, v2):
        accum += e1*e2
    return accum

################################################################
# Define action primitives to set the motor velocities for
# particular manipulandum motions.

def stop_motion(motors):
    for m in motors:
        m.setVelocity(0)

def random_motion(motors):
    for m in motors:
        m.setVelocity(4*random.random() - 2.0)

# Spin the object around the base center at the angular velocity
# specified in radians/sec.  N.B. 180 degrees == pi radians, 1
# radian is about 57 degrees.
def spin_around_center(motors, angvel=math.pi/12):
    motor_vel = -(angvel*base_radius)/wheel_radius
    print(f"All motors set to {motor_vel} rad/sec.")
    for m in motors:
        m.setVelocity(motor_vel)

# Move the object along a XYZ velocity vector expressed in
# meters/sec.  This take the dot product of the velocity vector
# with each wheel tangent vector to find the contribution of
# each wheel to the linear velocity, then scales by the wheel
# radius to each wheel rotational velocity.
def move_along_vector(motors, velvec):
    qd = [dot_product(velvec, tangent)/wheel_radius for tangent in wheel_tangents]
    for m, v in zip(motors, qd):
        m.setVelocity(v)
    print(f"Applying motor velocities {qd} rad/sec.")

# Move the object along the X axis at the linear velocity specified in
# meters/second.
def move_along_x(motors, vel=0.1):
    # Take the dot product of the velocity vector with each
    # wheel tangent vector to find the contribution of each
    # wheel to the linear velocity, then scale by the wheel
    # radius to find the rotational velocity.
    move_along_vector(motors, (vel, 0.0, 0.0))

# Move the object along the Y axis at the linear velocity specified in
# meters/second.
def move_along_y(motors, vel=0.1):
    # Take the dot product of the velocity vector with each
    # wheel tangent vector to find the contribution of each
    # wheel to the linear velocity, then scale by the wheel
    # radius to find the rotational velocity.
    move_along_vector(motors, (0.0, vel, 0.0))

    
################################################################
# Define a few global variables for the behavior state machine.
# This would be better represented as an object class.
state_index = 'start'
state_timer = 0
state_new_flag = False

# Utility function for state machine transitions.
def transition(next_state):
    global state_timer, state_index, state_new_flag
    state_index = next_state
    state_timer = 0.0
    state_new_flag = True
    print(f"Entering state {next_state}.")

################################################################    
# Run an event loop until the simulation quits,
# indicated by the step function returning -1.
while robot.step(EVENT_LOOP_DT) != -1:

    # Read simulator clock time.
    t = robot.getTime()

    # Read the new joint positions.
    q = [joint.getValue() for joint in sensors]

    # Evaluate a state machine to switch between action primitives.
    state_timer -= 0.001 * EVENT_LOOP_DT
    state_is_new = state_new_flag
    state_new_flag = False
    
    if state_index == 'start':
        transition('spinning1')

    elif state_index == 'spinning1':
        if state_is_new:
            state_timer = 2.0
            spin_around_center(motors, 0.2)
        elif state_timer < 0.0:
            transition('pause1')
            
    elif state_index == 'pause1':
        if state_is_new:
            state_timer = 1.0
            stop_motion(motors)
        elif state_timer < 0.0:
            transition('X+')

    elif state_index == 'X+':
        if state_is_new:
            move_along_x(motors, 0.1)
            state_timer = 0.5
        elif state_timer < 0.0:
            transition('X-')

    elif state_index == 'X-':
        if state_is_new:
            move_along_x(motors, -0.1)
            state_timer = 1.5
        elif state_timer < 0.0:
            transition('pause2')

    elif state_index == 'pause2':
        if state_is_new:
            state_timer = 1.0
            stop_motion(motors)
        elif state_timer < 0.0:
            transition('spinning2')

    elif state_index == 'spinning2':
        if state_is_new:
            spin_around_center(motors, -0.2)
            state_timer = 1.0
        elif state_timer < 0.0:
            transition('Y+')

    elif state_index == 'Y+':
        if state_is_new:            
            move_along_y(motors, 0.1)
            state_timer = 0.5
        elif state_timer < 0.0:
            transition('Y-')

    elif state_index == 'Y-':
        if state_is_new:                    
            move_along_y(motors, -0.2)
            state_timer = 1.5
        elif state_timer < 0.0:
            transition('pause3')

    elif state_index == 'pause3':
        if state_is_new:
            state_timer = 1.0
            stop_motion(motors)
        elif state_timer < 0.0:
            transition('spinning1')

World File

The robot is directly modeled in the scene tree so all parameters can be visible. In general it is more flexible to transfer models to .proto files so they can be instantiated more than once.

  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
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
#VRML_SIM R2021b utf8
WorldInfo {
}
Viewpoint {
  orientation 0.2994195144234726 0.4501928793641616 0.8412338115840431 2.18893833577019
  position 1.1687479458583148 0.5203935199223213 0.8904886319057939
}
Background {
  skyColor [
    0.1 0.1 0.1
  ]
}
DirectionalLight {
  direction 0.4 -0.5 -1
  intensity 3
  castShadows TRUE
}
RectangleArena {
  rotation 1 0 0 1.5708
  floorSize 2 2
}
Robot {
  rotation 0 0 1 1.5708
  children [
    DEF baseObject Transform {
      translation 0 0 0.05
      rotation 1 0 0 1.5708
      children [
        Shape {
          appearance PaintedWood {
            colorOverride 0.21529 0.543008 0.99855
          }
          geometry Cylinder {
            height 0.1
            radius 0.3
          }
        }
      ]
    }
    HingeJoint {
      jointParameters HingeJointParameters {
        anchor 0 0 0.1
      }
      device [
        PositionSensor {
          name "joint1"
        }
        RotationalMotor {
          name "motor1"
          acceleration 2
          maxVelocity 50
          minPosition -10
          maxPosition 10
          maxTorque 20
        }
      ]
      endPoint Solid {
        translation 0.3 0 0.1
        rotation 1 0 0 0
        children [
          DEF link1Shape Transform {
            translation 0.01 0 0
            rotation 0 0 1 -1.5707953071795862
            children [
              Shape {
                appearance PaintedWood {
                  colorOverride 0.990494 0.516915 0.468254
                }
                geometry Cylinder {
                  height 0.02
                  radius 0.05
                }
              }
            ]
          }
          Transform {
            translation 0.01 0 0
            rotation 0 0 1 -1.5707953071795862
            children [
              Shape {
                appearance PBRAppearance {
                  baseColor 0.511208 0.757198 0.00920119
                }
                geometry Box {
                  size 0.02 0.03 0.02
                }
              }
            ]
          }
        ]
        name "link1"
        boundingObject USE link1Shape
        physics Physics {
          density -1
          mass 0.5
        }
      }
    }
    HingeJoint {
      jointParameters HingeJointParameters {
        axis -0.5 0.866025 0
        anchor 0 0 0.1
      }
      device [
        PositionSensor {
          name "joint2"
        }
        RotationalMotor {
          name "motor2"
          acceleration 2
          maxVelocity 50
          minPosition -10
          maxPosition 10
          maxTorque 20
        }
      ]
      endPoint Solid {
        translation -0.15 0.259808 0.1
        rotation 0 0 1 2.0944
        children [
          DEF link2Shape Transform {
            translation 0.01 0 0
            rotation 0 0 1 -1.5707953071795862
            children [
              Shape {
                appearance PaintedWood {
                  colorOverride 0.990494 0.516915 0.468254
                }
                geometry Cylinder {
                  height 0.02
                  radius 0.05
                }
              }
            ]
          }
          Transform {
            translation 0.01 0 0
            rotation 0 0 1 -1.5707953071795862
            children [
              Shape {
                appearance PBRAppearance {
                  baseColor 0.511208 0.757198 0.00920119
                }
                geometry Box {
                  size 0.02 0.03 0.02
                }
              }
            ]
          }
        ]
        name "link2"
        boundingObject USE link2Shape
        physics Physics {
          density -1
          mass 0.5
        }
      }
    }
    HingeJoint {
      jointParameters HingeJointParameters {
        axis -0.5 -0.866025 0
        anchor 0 0 0.1
      }
      device [
        PositionSensor {
          name "joint3"
        }
        RotationalMotor {
          name "motor3"
          acceleration 2
          maxVelocity 50
          minPosition -10
          maxPosition 10
          maxTorque 20
        }
      ]
      endPoint Solid {
        translation -0.15 -0.259808 0.1
        rotation 0 0 1 4.1887902047863905
        children [
          DEF link3Shape Transform {
            translation 0.01 0 0
            rotation 0 0 1 -1.5707953071795862
            children [
              Shape {
                appearance PaintedWood {
                  colorOverride 0.990494 0.516915 0.468254
                }
                geometry Cylinder {
                  height 0.02
                  radius 0.05
                }
              }
            ]
          }
          Transform {
            translation 0.01 0 0
            rotation 0 0 1 -1.5707953071795862
            children [
              Shape {
                appearance PBRAppearance {
                  baseColor 0.511208 0.757198 0.00920119
                }
                geometry Box {
                  size 0.02 0.03 0.02
                }
              }
            ]
          }
        ]
        name "link3"
        boundingObject USE link3Shape
        physics Physics {
          density -1
          mass 0.5
        }
      }
    }
  ]
  name "conveyor"
  boundingObject USE baseObject
  controller "conveyor"
}
Solid {
  translation 0 0 0.17
  children [
    DEF manipShape Shape {
      appearance PBRAppearance {
        baseColor 0.341176 1 0.819608
        transparency 0.05
        roughness 0.3
      }
      geometry Box {
        size 0.8 0.8 0.025
      }
    }
  ]
  name "manipulandum"
  boundingObject USE manipShape
  physics Physics {
    density -1
    mass 2
  }
}