Pendulum 1-2 Robot Model

This sample project includes a model of a underactuated robot structured as a double pendulum with two distal links. The proximal link is actuated with a torque motor, and the distal links use passive pivots with only friction.

This model is demonstrated in the pendulum-1-2-demo.wbt world.

../_images/pendulum-1-2-sim.png

Screenshot of Webots model of 1-2 pendulum with a driven base joint and two passive distal joints. The robot is shown in the neutral pose.

System Kinematics

The bodies are as follows:

name

color

notes

base

blue

massive base resting on ground

link1

red

pivoting arm, with center of mass offset above the joint axis

link2A

green

upper passively pivoting arm,

link2B

green

lower passively pivoting arm, somewhat shorter than link2A

The joints are as follows:

name

parent

child

notes

joint1

base

link1

includes motor1 and the joint1 sensor

joint2A

link1

link2A

includes joint2A position sensor

joint2B

link1

link2A

includes joint2B position sensor

The axes are as follows:

name

direction

notes

joint1

along X

located 1.0 meters above the ground plane

joint2A

along X

located 0.5 meters about the joint1 axis

joint2B

along X

located 0.3 meters below the joint1 axis

Design Notes

  1. 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.

  2. For simplicity, the model uses only primitive shapes (boxes). The same shapes are used for both rendering and contact detection (i.e. bounding objects). This also allows the simulator to automatically calculate inertia properties.

  3. Each Solid node uses the translation vector to place the link body coordinate origin at a logical point on the joint axis. This convention is intended to keep the hinge vectors more legible.

  4. Each Shape node has been wrapped in a Transform node to ease positioning it with the link body coordinates.

  5. The sample controller runs an event loop which calculates joint1 torques based on elapsed time. It could be extended to use joint position data for feedback.

CAD Drawings

The following images were generated using a Fusion 360 CAD model independently drawn to the same dimensions as the Webots robot model.

../_images/pendulum-1-2-CAD-v3-dims.png

Dimensioned sketch of CAD model of 1-2 pendulum, front view, showing all X-Z dimensions.

../_images/pendulum-1-2-CAD-v3-oblique.png

CAD model of 1-2 pendulum.

../_images/pendulum-1-2-CAD-v3-front-right.png

CAD model of 1-2 pendulum, front and right views.

Sample Robot 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
54
55
56
57
58
59
60
61
62
63
# pendulum_1_2.py
#
# Sample Webots controller file for driving the
# underactuated 1-2 pendulum.  The robot has a driven
# base joint between the base and link1, and passive
# joints between link1 and the two distal links link2A
# and link2B.
#
# No copyright, 2020-2021, Garth Zeglin.  This file is
# explicitly placed in the public domain.

print("loading pendulum_1_2.py...")

# Import the Webots simulator API.
from controller import Robot

# Import the standard Python math library.
import math

print("pendulum_1_2.py waking up...")

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

# Request a proxy object representing the robot to
# control.
robot = Robot()

# Fetch handles for the joint sensors.
j1  = robot.getDevice('joint1')
j2A = robot.getDevice('joint2A')
j2B = robot.getDevice('joint2B')

# Specify the sampling rate for the joint sensors.
j1.enable(EVENT_LOOP_DT)
j2A.enable(EVENT_LOOP_DT)
j2B.enable(EVENT_LOOP_DT)

# Fetch handle for the 'base' joint motor.  In this
# example the motor will be controlled as a torque
# motor, bypassing the lower-level PID control.
motor1 = robot.getDevice('motor1')
motor1.setTorque(0.0)

# Run an event loop until the simulation quits,
# indicated by the step function returning -1.
while robot.step(EVENT_LOOP_DT) != -1:

    # Read simulator clock time.
    t = robot.getTime()

    # Read the new joint positions.
    q1  = j1.getValue()
    q2A = j2A.getValue()
    q2B = j2B.getValue()

    # Compute and apply new base joint actuator torque.
    # In this example, the excitation is only based on
    # time, but could also be a function of the joint
    # positions.
    tau = 3 * math.sin(3*t)
    motor1.setTorque(tau)

World File

  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
#VRML_SIM R2021b utf8
WorldInfo {
}
Viewpoint {
  orientation 0.1297234145059892 0.5459119370702683 0.8277390849132925 2.7234671260441967
  position 1.7870404774717816 3.2844836209794273 2.3418665277932647
}
Background {
  skyColor [
    0.1 0.1 0.1
  ]
}
DirectionalLight {
  direction 0.4 -0.5 -1
  intensity 3
  castShadows TRUE
}
RectangleArena {
  rotation 1 0 0 1.5708
  floorSize 2 2
}
Robot {
  rotation 0 0 1 1.5708
  children [
    DEF baseObject Transform {
      translation 0 0 0.6
      children [
        Shape {
          appearance PaintedWood {
            colorOverride 0.21529 0.543008 0.99855
          }
          geometry Box {
            size 0.3 0.5 1.2
          }
        }
      ]
    }
    HingeJoint {
      jointParameters HingeJointParameters {
        anchor 0 0 1
      }
      device [
        PositionSensor {
          name "joint1"
        }
        RotationalMotor {
          name "motor1"
          acceleration 2
          maxVelocity 3.14
          minPosition -10
          maxPosition 10
          maxTorque 20
        }
      ]
      endPoint Solid {
        translation 0.15 0 1
        rotation 1 0 0 0
        children [
          DEF link1Shape Transform {
            translation 0.08 0 0.1
            children [
              Shape {
                appearance PaintedWood {
                  colorOverride 0.990494 0.516915 0.468254
                }
                geometry Box {
                  size 0.1 0.3 1
                }
              }
            ]
          }
          HingeJoint {
            jointParameters HingeJointParameters {
              anchor 0 0 0.5
              dampingConstant 0.1
            }
            device [
              PositionSensor {
                name "joint2A"
              }
            ]
            endPoint Solid {
              translation 0.13 0 0.5
              rotation 1 0 0 0
              children [
                DEF link2AShape Transform {
                  translation 0.07 0 0.2
                  children [
                    Shape {
                      appearance PaintedWood {
                        colorOverride 0.413001 1 0.33489
                      }
                      geometry Box {
                        size 0.1 0.1 0.6
                      }
                    }
                  ]
                }
              ]
              name "link2A"
              boundingObject USE link2AShape
              physics Physics {
                density -1
                mass 0.5
              }
            }
          }
          HingeJoint {
            jointParameters HingeJointParameters {
              anchor 0 0 -0.3
              dampingConstant 0.1
            }
            device [
              PositionSensor {
                name "joint2B"
              }
            ]
            endPoint Solid {
              translation 0.13 0 -0.3
              rotation 1 0 0 0
              children [
                DEF link2BShape Transform {
                  translation 0.07 0 -0.15
                  children [
                    Shape {
                      appearance PaintedWood {
                        colorOverride 0.413001 1 0.33489
                      }
                      geometry Box {
                        size 0.1 0.1 0.4
                      }
                    }
                  ]
                }
              ]
              name "link2B"
              boundingObject USE link2BShape
              physics Physics {
                density -1
                mass 0.5
              }
            }
          }
        ]
        name "link1"
        boundingObject USE link1Shape
        physics Physics {
          density -1
          mass 0.5
        }
      }
    }
  ]
  name "pendulum_1_2"
  boundingObject USE baseObject
  physics Physics {
    density -1
    mass 20
  }
  controller "pendulum_1_2"
}