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-demo.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# conveyor.py
  2#
  3# Sample Webots controller file for driving the three-wheel
  4# conveyor demo.  The robot comprises three driven wheels
  5# arranged in a triangle.  A large passive object (the
  6# 'manipulandum') is balanced on top of the wheels for them to
  7# move.
  8#
  9# No copyright, 2021, Garth Zeglin.  This file is
 10# explicitly placed in the public domain.
 11
 12print("loading conveyor.py...")
 13
 14# Import the Webots simulator API.
 15from controller import Robot
 16
 17# Import standard Python modules
 18import math, random
 19
 20print("conveyor.py waking up...")
 21
 22# Define the time step in milliseconds between
 23# controller updates.
 24EVENT_LOOP_DT = 20
 25
 26# Request a proxy object representing the robot to
 27# control.
 28robot = Robot()
 29
 30# Fetch handles for the joint sensors.
 31sensors = (robot.getDevice('joint1'),
 32           robot.getDevice('joint2'),
 33           robot.getDevice('joint3'))
 34
 35# Specify the sampling rate for the joint sensors.
 36for s in sensors:
 37    s.enable(EVENT_LOOP_DT)
 38
 39# Fetch handle for each conveyor wheel motor.  The motor1 axis
 40# is positioned along X, so rotations will move the object along
 41# Y.  Motor2 is 120 degrees CCW from motor1.
 42motors = (robot.getDevice('motor1'),
 43          robot.getDevice('motor2'),
 44          robot.getDevice('motor3'))
 45
 46# Define a few constant which match the model to be used for
 47# kinematics calculations.  All units are meters to match the
 48# simulation.
 49base_radius  = 0.3
 50wheel_radius = 0.05
 51
 52wheel_origins = ((0.3, 0.0, 0.1),
 53                 (-0.15,0.259808, 0.1),
 54                 (-0.15,-0.259808,0.1))
 55
 56wheel_axes    = ((1.0,0.0,0.0),
 57                 (-0.5,0.866025,0),
 58                 (-0.5,-0.866025,0))
 59
 60# The wheel tangent axes define the direction of travel of the
 61# top contact point for a positive wheel rotation.
 62wheel_tangents = ((0.0,-1.0,0.0),
 63                  (0.866025, 0.5,0),
 64                  (-0.866025, 0.5,0))
 65
 66# In this example the motor will use velocity control,
 67# configured by setting an infinite position target and then
 68# using setVelocity() to issue motor commands.
 69for motor in motors:
 70    motor.setPosition(math.inf)
 71    motor.setVelocity(0)
 72
 73################################################################
 74# Dot product utility function.  It would be better to use
 75# numpy, but this avoids adding the dependency.
 76def dot_product(v1, v2):
 77    accum = 0.0
 78    for e1, e2 in zip(v1, v2):
 79        accum += e1*e2
 80    return accum
 81
 82################################################################
 83# Define action primitives to set the motor velocities for
 84# particular manipulandum motions.
 85
 86def stop_motion(motors):
 87    for m in motors:
 88        m.setVelocity(0)
 89
 90def random_motion(motors):
 91    for m in motors:
 92        m.setVelocity(4*random.random() - 2.0)
 93
 94# Spin the object around the base center at the angular velocity
 95# specified in radians/sec.  N.B. 180 degrees == pi radians, 1
 96# radian is about 57 degrees.
 97def spin_around_center(motors, angvel=math.pi/12):
 98    motor_vel = -(angvel*base_radius)/wheel_radius
 99    print(f"All motors set to {motor_vel} rad/sec.")
100    for m in motors:
101        m.setVelocity(motor_vel)
102
103# Move the object along a XYZ velocity vector expressed in
104# meters/sec.  This take the dot product of the velocity vector
105# with each wheel tangent vector to find the contribution of
106# each wheel to the linear velocity, then scales by the wheel
107# radius to each wheel rotational velocity.
108def move_along_vector(motors, velvec):
109    qd = [dot_product(velvec, tangent)/wheel_radius for tangent in wheel_tangents]
110    for m, v in zip(motors, qd):
111        m.setVelocity(v)
112    print(f"Applying motor velocities {qd} rad/sec.")
113
114# Move the object along the X axis at the linear velocity specified in
115# meters/second.
116def move_along_x(motors, vel=0.1):
117    # Take the dot product of the velocity vector with each
118    # wheel tangent vector to find the contribution of each
119    # wheel to the linear velocity, then scale by the wheel
120    # radius to find the rotational velocity.
121    move_along_vector(motors, (vel, 0.0, 0.0))
122
123# Move the object along the Y axis at the linear velocity specified in
124# meters/second.
125def move_along_y(motors, vel=0.1):
126    # Take the dot product of the velocity vector with each
127    # wheel tangent vector to find the contribution of each
128    # wheel to the linear velocity, then scale by the wheel
129    # radius to find the rotational velocity.
130    move_along_vector(motors, (0.0, vel, 0.0))
131
132    
133################################################################
134# Define a few global variables for the behavior state machine.
135# This would be better represented as an object class.
136state_index = 'start'
137state_timer = 0
138state_new_flag = False
139
140# Utility function for state machine transitions.
141def transition(next_state):
142    global state_timer, state_index, state_new_flag
143    state_index = next_state
144    state_timer = 0.0
145    state_new_flag = True
146    print(f"Entering state {next_state}.")
147
148################################################################    
149# Run an event loop until the simulation quits,
150# indicated by the step function returning -1.
151while robot.step(EVENT_LOOP_DT) != -1:
152
153    # Read simulator clock time.
154    t = robot.getTime()
155
156    # Read the new joint positions.
157    q = [joint.getValue() for joint in sensors]
158
159    # Evaluate a state machine to switch between action primitives.
160    state_timer -= 0.001 * EVENT_LOOP_DT
161    state_is_new = state_new_flag
162    state_new_flag = False
163    
164    if state_index == 'start':
165        transition('spinning1')
166
167    elif state_index == 'spinning1':
168        if state_is_new:
169            state_timer = 2.0
170            spin_around_center(motors, 0.2)
171        elif state_timer < 0.0:
172            transition('pause1')
173            
174    elif state_index == 'pause1':
175        if state_is_new:
176            state_timer = 1.0
177            stop_motion(motors)
178        elif state_timer < 0.0:
179            transition('X+')
180
181    elif state_index == 'X+':
182        if state_is_new:
183            move_along_x(motors, 0.1)
184            state_timer = 0.5
185        elif state_timer < 0.0:
186            transition('X-')
187
188    elif state_index == 'X-':
189        if state_is_new:
190            move_along_x(motors, -0.1)
191            state_timer = 1.5
192        elif state_timer < 0.0:
193            transition('pause2')
194
195    elif state_index == 'pause2':
196        if state_is_new:
197            state_timer = 1.0
198            stop_motion(motors)
199        elif state_timer < 0.0:
200            transition('spinning2')
201
202    elif state_index == 'spinning2':
203        if state_is_new:
204            spin_around_center(motors, -0.2)
205            state_timer = 1.0
206        elif state_timer < 0.0:
207            transition('Y+')
208
209    elif state_index == 'Y+':
210        if state_is_new:            
211            move_along_y(motors, 0.1)
212            state_timer = 0.5
213        elif state_timer < 0.0:
214            transition('Y-')
215
216    elif state_index == 'Y-':
217        if state_is_new:                    
218            move_along_y(motors, -0.2)
219            state_timer = 1.5
220        elif state_timer < 0.0:
221            transition('pause3')
222
223    elif state_index == 'pause3':
224        if state_is_new:
225            state_timer = 1.0
226            stop_motion(motors)
227        elif state_timer < 0.0:
228            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#VRML_SIM R2022a utf8
  2WorldInfo {
  3}
  4Viewpoint {
  5  orientation 0.2762210002867493 0.07933708465413124 -0.9578139621028559 2.715308734468781
  6  position 1.1687479458583148 0.5203935199223213 0.8904886319057939
  7}
  8Background {
  9  skyColor [
 10    0.1 0.1 0.1
 11  ]
 12}
 13DirectionalLight {
 14  direction 0.4 -0.5 -1
 15  intensity 3
 16  castShadows TRUE
 17}
 18RectangleArena {
 19  rotation 1.8366025517039032e-06 -1.836602551703903e-06 0.9999999999966269 1.5707963267982696
 20  floorSize 2 2
 21}
 22Robot {
 23  rotation 0 0 1 1.5708
 24  children [
 25    DEF baseObject Transform {
 26      translation 0 0 0.05
 27      rotation 0.9999999999999999 0 0 3.6731891284930826e-06
 28      children [
 29        Shape {
 30          appearance PaintedWood {
 31            colorOverride 0.21529 0.543008 0.99855
 32          }
 33          geometry Cylinder {
 34            height 0.1
 35            radius 0.3
 36          }
 37        }
 38      ]
 39    }
 40    HingeJoint {
 41      jointParameters HingeJointParameters {
 42        anchor 0 0 0.1
 43      }
 44      device [
 45        PositionSensor {
 46          name "joint1"
 47        }
 48        RotationalMotor {
 49          name "motor1"
 50          acceleration 2
 51          maxVelocity 50
 52          minPosition -10
 53          maxPosition 10
 54          maxTorque 20
 55        }
 56      ]
 57      endPoint Solid {
 58        translation 0.3 0 0.1
 59        rotation 1 0 0 0
 60        children [
 61          DEF link1Shape Transform {
 62            translation 0.01 0 0
 63            rotation -0.5773506616397417 0.5773500729644677 -0.5773500729644677 2.0943945137181217
 64            children [
 65              Shape {
 66                appearance PaintedWood {
 67                  colorOverride 0.990494 0.516915 0.468254
 68                }
 69                geometry Cylinder {
 70                  height 0.02
 71                  radius 0.05
 72                }
 73              }
 74            ]
 75          }
 76          Transform {
 77            translation 0.01 0 0
 78            rotation 0 0 1 -1.5707953071795862
 79            children [
 80              Shape {
 81                appearance PBRAppearance {
 82                  baseColor 0.511208 0.757198 0.00920119
 83                }
 84                geometry Box {
 85                  size 0.02 0.03 0.02
 86                }
 87              }
 88            ]
 89          }
 90        ]
 91        name "link1"
 92        boundingObject USE link1Shape
 93        physics Physics {
 94          density -1
 95          mass 0.5
 96        }
 97      }
 98    }
 99    HingeJoint {
100      jointParameters HingeJointParameters {
101        axis -0.5 0.866025 0
102        anchor 0 0 0.1
103      }
104      device [
105        PositionSensor {
106          name "joint2"
107        }
108        RotationalMotor {
109          name "motor2"
110          acceleration 2
111          maxVelocity 50
112          minPosition -10
113          maxPosition 10
114          maxTorque 20
115        }
116      ]
117      endPoint Solid {
118        translation -0.15 0.259808 0.1
119        rotation 0 0 1 2.0944
120        children [
121          DEF link2Shape Transform {
122            translation 0.01 0 0
123            rotation -0.5773506616397417 0.5773500729644677 -0.5773500729644677 2.0943945137181217
124            children [
125              Shape {
126                appearance PaintedWood {
127                  colorOverride 0.990494 0.516915 0.468254
128                }
129                geometry Cylinder {
130                  height 0.02
131                  radius 0.05
132                }
133              }
134            ]
135          }
136          Transform {
137            translation 0.01 0 0
138            rotation 0 0 1 -1.5707953071795862
139            children [
140              Shape {
141                appearance PBRAppearance {
142                  baseColor 0.511208 0.757198 0.00920119
143                }
144                geometry Box {
145                  size 0.02 0.03 0.02
146                }
147              }
148            ]
149          }
150        ]
151        name "link2"
152        boundingObject USE link2Shape
153        physics Physics {
154          density -1
155          mass 0.5
156        }
157      }
158    }
159    HingeJoint {
160      jointParameters HingeJointParameters {
161        axis -0.5 -0.866025 0
162        anchor 0 0 0.1
163      }
164      device [
165        PositionSensor {
166          name "joint3"
167        }
168        RotationalMotor {
169          name "motor3"
170          acceleration 2
171          maxVelocity 50
172          minPosition -10
173          maxPosition 10
174          maxTorque 20
175        }
176      ]
177      endPoint Solid {
178        translation -0.15 -0.259808 0.1
179        rotation 0 0 1 4.1887902047863905
180        children [
181          DEF link3Shape Transform {
182            translation 0.01 0 0
183            rotation -0.5773506616397417 0.5773500729644677 -0.5773500729644677 2.0943945137181217
184            children [
185              Shape {
186                appearance PaintedWood {
187                  colorOverride 0.990494 0.516915 0.468254
188                }
189                geometry Cylinder {
190                  height 0.02
191                  radius 0.05
192                }
193              }
194            ]
195          }
196          Transform {
197            translation 0.01 0 0
198            rotation 0 0 1 -1.5707953071795862
199            children [
200              Shape {
201                appearance PBRAppearance {
202                  baseColor 0.511208 0.757198 0.00920119
203                }
204                geometry Box {
205                  size 0.02 0.03 0.02
206                }
207              }
208            ]
209          }
210        ]
211        name "link3"
212        boundingObject USE link3Shape
213        physics Physics {
214          density -1
215          mass 0.5
216        }
217      }
218    }
219  ]
220  name "conveyor"
221  boundingObject USE baseObject
222  controller "conveyor"
223}
224Solid {
225  translation 0 0 0.17
226  children [
227    DEF manipShape Shape {
228      appearance PBRAppearance {
229        baseColor 0.341176 1 0.819608
230        transparency 0.05
231        roughness 0.3
232      }
233      geometry Box {
234        size 0.8 0.8 0.025
235      }
236    }
237  ]
238  name "manipulandum"
239  boundingObject USE manipShape
240  physics Physics {
241    density -1
242    mass 2
243  }
244}