Robotics for Creative Practice - Fall 2022

Two-Link Robot Model

«  Pendulum 1-2 Robot Model   ::   Contents   ::   Clock Robot Model  »

Two-Link Robot Model¶

The two-link robot model simulates a planar two-link robot with an actuator at each joint and a sensor at the end. The model is intended as a demonstration testbed for two-link kinematics.

The link geometry uses only cylinder primitives. The same geometry is referenced to use as bounding objects for collision and automatic calculation of physics parameters. The base object has a NULL Physics object so it does not move, simulating a rigid connection to the ground.

The end sensor is represented by a yellow cone. It is implemented as a DistanceSensor of limited range, pointing outward along the link axis from the end of the second link.

This model is demonstrated in the sensor-demo.wbt and controls-demo.wbt worlds.

../_images/two-link-sim.jpg

Screenshot of Webots model of fully-actuated two-link planar robot.¶

Contents

  • Two-Link Robot Model

    • System Kinematics

    • two-link.proto

    • Sample Control Code

System Kinematics¶

The bodies are as follows:

name

color

notes

base

blue

base object fixed to the ground

link1

red

proximal link, attaches to base at ‘shoulder’

link2

green

the distal link, attaches to link1 at ‘elbow’

The joints are as follows:

name

parent

child

notes

joint1

base

link1

the ‘shoulder’, includes motor1

joint2

link1

link2

the ‘elbow’, includes motor2

The axes are as follows:

name

direction

notes

joint1

along Z

located above the origin

joint2

along Z

located at the end of link1

The motors and sensors are named as follows:

name

notes

motor1

RotationalMotor on joint1

motor2

RotationalMotor on joint1

joint1

PositionSensor on joint1

joint2

PositionSensor on joint2

endRangeSensor

DistanceSensor at end of link2

two-link.proto¶

The robot model has been encapsulated in a .proto file for easy reuse. The model includes user-accessible link length parameters to demonstrate procedural scaling.

  1#VRML_SIM R2022a utf8
  2# documentation url: https://courses.ideate.cmu.edu/16-375
  3# Planar two-link actuated arm for course exercises.  The graphics use only
  4# primitives for clarity of the source. The base has NULL physics so it will be
  5# fixed in place. The two link lengths are adjustable parameters to demonstrate
  6# using procedural elements in the prototype.  The link physics properties are
  7# specified using density so the dynamics will also scale, but the motor
  8# parameters are constant.  The end includes a distance sensor pointed along the
  9# axis.
 10# license: No copyright, 2020-2022 Garth Zeglin.  This file is explicitly placed in the public domain.
 11PROTO two-link [
 12  field SFVec3f    translation  0 0 0
 13  field SFRotation rotation     0 1 0 0
 14  field SFFloat    link1Length  0.5
 15  field SFFloat    link2Length  0.5
 16  field SFString   controller   "two_link"
 17  field SFString   name         ""
 18  field SFString   customData   ""
 19]
 20{
 21  Robot {
 22    # connect properties to user-visible data fields
 23    translation IS translation
 24    rotation IS rotation
 25    controller IS controller
 26    name IS name
 27    customData IS customData
 28
 29    # calculate derived parameters
 30    %{
 31      local halfLink1Len = fields.link1Length.value / 2
 32      local halfLink2Len = fields.link2Length.value / 2
 33    }%
 34
 35    # define the kinematic tree
 36    children [
 37      # add a default radio receiver and transmitter
 38      Receiver {
 39      }
 40      Emitter {
 41      }
 42
 43      # the cylindrical base shape is wrapped in a Transform
 44      # to position it within the robot body coordinates
 45      DEF baseObject Transform {
 46        translation 0 0 0.1
 47        children [
 48          Shape {
 49            appearance PBRAppearance {
 50              baseColor 0.21529 0.543008 0.99855
 51              metalness 0
 52            }
 53            geometry Cylinder {
 54	      height 0.2
 55	      radius 0.2
 56            }
 57          }
 58        ]
 59      }
 60      # define the base pivot joint connecting the base
 61      # and the first link
 62      HingeJoint {
 63        jointParameters HingeJointParameters {
 64          axis 0 0 1
 65        }
 66        device [
 67          PositionSensor {
 68            name "joint1"
 69          }
 70          RotationalMotor {
 71            name "motor1"
 72            acceleration 2
 73            maxVelocity 3.14
 74            maxTorque 2
 75          }
 76        ]
 77        # start definition of the first link
 78        endPoint Solid {
 79          # place the shape origin halfway along the first link;
 80          # this vector is in body coordinates, X points along
 81          # the link in the neutral pose
 82          translation %{=halfLink1Len}% 0 0.25
 83          children [
 84            # define the 'elbow' pivot connecting the links
 85            HingeJoint {
 86              jointParameters HingeJointParameters {
 87                axis 0 0 1
 88                # place the elbow joint axis at the end of the first
 89                # link; position is relative to link1 origin
 90                anchor %{= halfLink1Len }% 0 0
 91                dampingConstant 0.1
 92              }
 93              device [
 94                PositionSensor {
 95                  name "joint2"
 96                }
 97                RotationalMotor {
 98                name "motor2"
 99                acceleration 2
100                maxVelocity 6.28
101                maxTorque 1.5
102                }
103              ]
104              # define the second link
105              endPoint Solid {
106                # place the link2 origin halfway along the second link
107                translation %{= halfLink1Len+halfLink2Len}% 0 0.1
108                children [
109                  # the cylindrical link shape is wrapped in a Transform
110                  # to position it within the link2 coordinates
111                  DEF link2Shape Transform {
112                    # the Cylinder shape coordinates use Z as the
113                    # long axis; this 90 deg rotation around Y
114                    # places the lengthwise Z axis along the link.
115                    rotation 0 1 0 1.5708
116                    children [
117                      Shape {
118                        appearance DEF greenAppearance PBRAppearance {
119                          baseColor 0.413001 1 0.33489
120                          metalness 0
121                        }
122                        geometry Cylinder {
123                          height IS link2Length
124                          radius 0.05
125                        }
126                      }
127                   ]
128                 } # end link2 Shape
129	     	 # add a visual hub to the base of link2, not part of the bounding object
130	     	 Transform {
131             	    rotation 0 1 0 0
132             	    translation %{= -halfLink2Len}% 0 0
133  	     	 	children [
134	     	 	  Shape {
135             	        appearance USE greenAppearance
136             	        geometry Cylinder {
137	     	 	      height 0.1
138	     	 	      radius 0.05
139	     	 	    }
140	     	 	  }
141	     	 	]
142             	 } # end Transform around link2 base hub
143		 # define a DistanceSensor attached to the second link Solid node
144                 DistanceSensor {
145                   translation %{= halfLink2Len}% 0 0
146                   name "endRangeSensor"
147
148		   # the sensor lookup table implicitly defines the maximum range and the units, each
149		   # entry is [distance, value, noise]
150                   lookupTable [
151                     0 0 0
152                     0.9 0.9 0    # 0.9 meters reads as 0.9 meters
153                   ]
154		   resolution 0.001 # assume millimeter resolution
155                   numberOfRays 5
156                   aperture 0.1
157                   children [
158                     Transform {
159                       rotation 0 -1 0 1.5708
160                       children [
161                         Shape {
162                           appearance PBRAppearance {
163                             baseColor 1 0.99028 0.0584421
164                             roughness 0.5
165                             metalness 0.5
166                             emissiveColor 1 0.99028 0.0584421
167                             emissiveIntensity 0.2
168                           }
169                           geometry Cone {
170                             bottomRadius 0.02
171                             height 0.1
172                           }
173                         }
174                       ]
175                     }
176                   ]
177                 } # end DistanceSensor
178                ] # end link2 Solid children
179                # top-level properties of link2
180                name "link2"
181                boundingObject USE link2Shape
182                physics Physics {
183                  # Assume the link is a thin-walled aluminum tube with 50 mm
184		  # radius and 2 mm wall thickness.  Aluminum has a density of
185		  # 2700 kg/m^3, but this will be scaled by the ratio of the
186		  # tube cross-section to the solid cylinder cross-section
187		  # assumed by the simulator.  Note that the moment of inertia
188		  # around the long axis will be underestimated.
189		  # density = 2700 * (R_outer**2 - R_inner**2) / R_outer**2
190                  density 211.7
191                  mass -1
192                }
193              }
194            }
195            # finish the definition of link1 with a shape
196            # node in the 'children' list
197            DEF link1Shape Transform {
198              rotation 0 1 0 1.5708
199              children [
200                Shape {
201                  appearance DEF redAppearance PBRAppearance {
202                     baseColor 00.990494 0.516915 0.468254
203                     metalness 0
204                  }
205                  geometry Cylinder {
206                    height IS link1Length
207                    radius 0.05
208                  }
209                }
210               ]
211             }
212	     # add a visual hub to the base of link1, not part of the bounding object
213	     Transform {
214                rotation 1 0 0 0
215                translation %{= -halfLink1Len}% 0 0
216  		children [
217		  Shape {
218                    appearance USE redAppearance
219                    geometry Cylinder {
220		      height 0.1
221		      radius 0.05
222		    }
223		  }
224		]
225             } # end Transform around link1 base hub
226	     # add a visual hub to the end of link1, not part of the bounding object
227	     Transform {
228                rotation 1 0 0 0
229                translation %{= halfLink1Len}% 0 0
230  		children [
231		  Shape {
232                    appearance USE redAppearance
233                    geometry Cylinder {
234		      height 0.1
235		      radius 0.05
236		    }
237		  }
238		]
239             } # end Transform around link1 end hub
240          ] # close the children list of the link1 node
241          # top-level properties of link1
242          name "link1"
243          boundingObject USE link1Shape
244          physics Physics {
245	    # See notes for link2 density; this assumes the same geometry.
246            density 211.7
247            mass -1
248          }
249        }
250      }
251    ] # close the children list of the base node
252    # define top-level properties of the base
253    boundingObject USE baseObject
254
255    # the base of the robot itself has NULL physics to simulate being fixed to the ground
256    # physics Physics { density -1 mass 10  }
257
258  } # close the Robot definition
259}

Sample Control Code¶

 1# two_link.py
 2#
 3# Sample Webots controller file for driving the two-link arm
 4# with two driven joints.  This example simulates a passive
 5# distal link by applying zero torque, then moves the
 6# base joint in a periodic excitation.
 7
 8# No copyright, 2020-2021, Garth Zeglin.  This file is
 9# explicitly placed in the public domain.
10
11print("loading two_link.py...")
12
13# Import the Webots simulator API.
14from controller import Robot
15
16# Define the time step in milliseconds between controller updates.
17EVENT_LOOP_DT = 200
18
19# Request a proxy object representing the robot to control.
20robot = Robot()
21robot_name = robot.getName()
22print("%s: controller connected." % (robot_name))
23
24# Fetch handle for the 'base' and 'elbow' joint motors.
25j1 = robot.getDevice('motor1')
26j2 = robot.getDevice('motor2')
27
28# Configure the motor for velocity control by setting
29# the position targets to infinity.
30j1.setPosition(float('inf'))
31
32# Start out with a 3 radian/second target rotational
33# velocity (roughly 180 deg/sec).
34j1.setVelocity(3)
35
36# Configure the second motor to freewheel.  Please note
37# this does not turn off the hinge friction.  For reference see:
38#  https://cyberbotics.com/doc/reference/motor
39#  https://cyberbotics.com/doc/reference/rotationalmotor
40j2.setTorque(0.0)
41
42# Run loop to execute a periodic script until the simulation quits.
43# If the controller returns -1, the simulator is quitting.
44while robot.step(EVENT_LOOP_DT) != -1:
45    # Read simulator clock time.
46    t = robot.getTime()
47
48    # Change the target velocity in a cycle with a two-second period.
49    if int(t) % 2 == 0:
50        j1.setVelocity(0)
51    else:
52        j1.setVelocity(3)
53        

«  Pendulum 1-2 Robot Model   ::   Contents   ::   Clock Robot Model  »

© Copyright 2022, Garth Zeglin. Licensed under CC-BY-4.0. Last updated on 2022-12-08. Created using Sphinx 5.1.1. University legal notice.