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.
Screenshot of Webots model of fully-actuated two-link planar robot.¶
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 255 256 257 258 259 260 261 262 | #VRML_SIM R2021b 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-2021 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 ""
field SFString customData ""
]
{
Robot {
# connect properties to user-visible data fields
translation IS translation
rotation IS rotation
controller IS controller
name IS name
customData IS customData
# calculate derived parameters
%{
local halfLink1Len = fields.link1Length.value / 2
local halfLink2Len = fields.link2Length.value / 2
}%
# define the kinematic tree
children [
# add a default radio receiver and transmitter
Receiver {
}
Emitter {
}
# 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-2021, Garth Zeglin. This file is
# explicitly placed in the public domain.
print("loading two_link.py...")
# 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.getDevice('motor1')
j2 = robot.getDevice('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)
|