Robotics for Creative Practice - Fall 2020

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.

The model appears in the ‘controls’ project available as a zip file controls.zip.

../_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
  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
245
246
247
248
249
250
251
252
253
254
#VRML_SIM R2020b utf8
# documentation url: https://courses.ideate.cmu.edu/16-375
# Planar two-link actuated arm for course exercises.  The graphics use only
# primitives for clarity of the source. The base has NULL physics so it will be
# fixed in place. The two link lengths are adjustable parameters to demonstrate
# using procedural elements in the prototype.  The link physics properties are
# specified using density so the dynamics will also scale, but the motor
# parameters are constant.  The end includes a distance sensor pointed along the
# axis.
# license: No copyright, 2020 Garth Zeglin.  This file is explicitly placed in the public domain.
PROTO two-link [
  field SFVec3f    translation  0 0 0
  field SFRotation rotation     0 1 0 0
  field SFFloat    link1Length  0.5
  field SFFloat    link2Length  0.5
  field SFString   controller   "two_link"
  field SFString   name         ""
]
{
  Robot {
    # connect properties to user-visible data fields
    translation IS translation
    rotation IS rotation
    controller IS controller
    name IS name

    # calculate derived parameters
    %{
      local halfLink1Len = fields.link1Length.value / 2
      local halfLink2Len = fields.link2Length.value / 2
    }%

    # define the kinematic tree
    children [
      # the rectilinear base shape is wrapped in a Transform
      # to position it within the robot body coordinates
      DEF baseObject Transform {
        translation 0 0 0.1
        # the Cylinder shape coordinates use Y as the
        # long axis; this rotation places the axis vertically
        rotation 1 0 0 1.5708
        children [
          Shape {
            appearance PBRAppearance {
              baseColor 0.21529 0.543008 0.99855
              metalness 0
            }
            geometry Cylinder {
	      height 0.2
	      radius 0.2
            }
          }
        ]
      }
      # define the base pivot joint connecting the base
      # and the first link
      HingeJoint {
        jointParameters HingeJointParameters {
          axis 0 0 1
        }
        device [
          PositionSensor {
            name "joint1"
          }
          RotationalMotor {
            name "motor1"
            acceleration 2
            maxVelocity 3.14
            maxTorque 2
          }
        ]
        # start definition of the first link
        endPoint Solid {
          # place the shape origin halfway along the first link;
          # this vector is in body coordinates, X points along
          # the link in the neutral pose
          translation %{=halfLink1Len}% 0 0.25
          children [
            # define the 'elbow' pivot connecting the links
            HingeJoint {
              jointParameters HingeJointParameters {
                axis 0 0 1
                # place the elbow joint axis at the end of the first
                # link; position is relative to link1 origin
                anchor %{= halfLink1Len }% 0 0
                dampingConstant 0.1
              }
              device [
                PositionSensor {
                  name "joint2"
                }
                RotationalMotor {
                name "motor2"
                acceleration 2
                maxVelocity 6.28
                maxTorque 1.5
                }
              ]
              # define the second link
              endPoint Solid {
                # place the link2 origin halfway along the second link
                translation %{= halfLink1Len+halfLink2Len}% 0 0.1
                children [
                  # the cylindrical link shape is wrapped in a Transform
                  # to position it within the link2 coordinates
                  DEF link2Shape Transform {
                    # the Cylinder shape coordinates use Y as the
                    # long axis; this -90 deg rotation around Z
                    # places the lengthwise Y axis along the link.
                    rotation 0 0 -1 1.5708
                    children [
                      Shape {
                        appearance DEF greenAppearance PBRAppearance {
                          baseColor 0.413001 1 0.33489
                          metalness 0
                        }
                        geometry Cylinder {
                          height IS link2Length
                          radius 0.05
                        }
                      }
                   ]
                 } # end link2 Shape
	     	 # add a visual hub to the base of link2, not part of the bounding object
	     	 Transform {
             	    rotation 1 0 0 1.5708
             	    translation %{= -halfLink2Len}% 0 0
  	     	 	children [
	     	 	  Shape {
             	        appearance USE greenAppearance
             	        geometry Cylinder {
	     	 	      height 0.1
	     	 	      radius 0.05
	     	 	    }
	     	 	  }
	     	 	]
             	 } # end Transform around link2 base hub
		 # define a DistanceSensor attached to the second link Solid node
                 DistanceSensor {
                   translation %{= halfLink2Len}% 0 0
                   name "endRangeSensor"

		   # the sensor lookup table implicitly defines the maximum range and the units, each
		   # entry is [distance, value, noise]
                   lookupTable [
                     0 0 0
                     0.9 0.9 0    # 0.9 meters reads as 0.9 meters
                   ]
		   resolution 0.001 # assume millimeter resolution
                   numberOfRays 5
                   aperture 0.1
                   children [
                     Transform {
                       rotation 0 0 1 1.5708
                       children [
                         Shape {
                           appearance PBRAppearance {
                             baseColor 1 0.99028 0.0584421
                             roughness 0.5
                             metalness 0.5
                             emissiveColor 1 0.99028 0.0584421
                             emissiveIntensity 0.2
                           }
                           geometry Cone {
                             bottomRadius 0.02
                             height 0.1
                           }
                         }
                       ]
                     }
                   ]
                 } # end DistanceSensor
                ] # end link2 Solid children
                # top-level properties of link2
                name "link2"
                boundingObject USE link2Shape
                physics Physics {
                  # Assume the link is a thin-walled aluminum tube with 50 mm
		  # radius and 2 mm wall thickness.  Aluminum has a density of
		  # 2700 kg/m^3, but this will be scaled by the ratio of the
		  # tube cross-section to the solid cylinder cross-section
		  # assumed by the simulator.  Note that the moment of inertia
		  # around the long axis will be underestimated.
		  # density = 2700 * (R_outer**2 - R_inner**2) / R_outer**2
                  density 211.7
                  mass -1
                }
              }
            }
            # finish the definition of link1 with a shape
            # node in the 'children' list
            DEF link1Shape Transform {
              rotation 0 0 -1 1.5708
              children [
                Shape {
                  appearance DEF redAppearance PBRAppearance {
                     baseColor 00.990494 0.516915 0.468254
                     metalness 0
                  }
                  geometry Cylinder {
                    height IS link1Length
                    radius 0.05
                  }
                }
               ]
             }
	     # add a visual hub to the base of link1, not part of the bounding object
	     Transform {
                rotation 1 0 0 1.5708
                translation %{= -halfLink1Len}% 0 0
  		children [
		  Shape {
                    appearance USE redAppearance
                    geometry Cylinder {
		      height 0.1
		      radius 0.05
		    }
		  }
		]
             } # end Transform around link1 base hub
	     # add a visual hub to the end of link1, not part of the bounding object
	     Transform {
                rotation 1 0 0 1.5708
                translation %{= halfLink1Len}% 0 0
  		children [
		  Shape {
                    appearance USE redAppearance
                    geometry Cylinder {
		      height 0.1
		      radius 0.05
		    }
		  }
		]
             } # end Transform around link1 end hub
          ] # close the children list of the link1 node
          # top-level properties of link1
          name "link1"
          boundingObject USE link1Shape
          physics Physics {
	    # See notes for link2 density; this assumes the same geometry.
            density 211.7
            mass -1
          }
        }
      }
    ] # close the children list of the base node
    # define top-level properties of the base
    boundingObject USE baseObject

    # the base of the robot itself has NULL physics to simulate being fixed to the ground
    # physics Physics { density -1 mass 10  }

  } # close the Robot definition
}

Sample 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
# two_link.py
#
# Sample Webots controller file for driving the two-link arm
# with two driven joints.  This example simulates a passive
# distal link by applying zero torque, then moves the
# base joint in a periodic excitation.

# No copyright, 2020, Garth Zeglin.  This file is
# explicitly placed in the public domain.

print("two_link.py waking up.")

# Import the Webots simulator API.
from controller import Robot

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

# Request a proxy object representing the robot to control.
robot = Robot()
robot_name = robot.getName()
print("%s: controller connected." % (robot_name))

# Fetch handle for the 'base' and 'elbow' joint motors.
j1 = robot.getMotor('motor1')
j2 = robot.getMotor('motor2')

# Configure the motor for velocity control by setting
# the position targets to infinity.
j1.setPosition(float('inf'))

# Start out with a 3 radian/second target rotational
# velocity (roughly 180 deg/sec).
j1.setVelocity(3)

# Configure the second motor to freewheel.  Please note
# this does not turn off the hinge friction.  For reference see:
#  https://cyberbotics.com/doc/reference/motor
#  https://cyberbotics.com/doc/reference/rotationalmotor
j2.setTorque(0.0)

# Run loop to execute a periodic script until the simulation quits.
# If the controller returns -1, the simulator is quitting.
while robot.step(EVENT_LOOP_DT) != -1:
    # Read simulator clock time.
    t = robot.getTime()

    # Change the target velocity in a cycle with a two-second period.
    if int(t) % 2 == 0:
        j1.setVelocity(0)
    else:
        j1.setVelocity(3)

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

© Copyright 2020, Garth Zeglin. Licensed under CC-BY-4.0. Last updated on 2021-01-26. Created using Sphinx 3.0.4. University legal notice.