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