Boom-Monopod Model¶
The boom-monopod model simulates elements of legged locomotion using a single-legged hopping device on the end of a planarizing boom.
The base of the joint includes an unactuated universal joint which allow vertical boom motion and ‘forward’ travel in a circle around the base. In the reference pose, the boom is slanted to keep the base pivots low to the floor. Placing the pivot center near the contact plane minimizes radial foot motion during stance. At the end of boom is a nominally horizontal axis to allow body pitch.
In this implementation, the body pitch axis is actuated to allow controlling the leg angle with respect to the floor. This makes control much easier but diverges from an approximation of free locomotion.
The default controller produces a hopping motion at controlled speed. The leg spring is simulated using a linear leg actuator in force control mode. Constant thrust is provided by changing the spring setpoint during the takeoff portion of the gait cycle. The speed control applies linear feedback to the leg angle to adjust estimated speed to the reference speed.
Note: simulating the spring in the controller requires setting the WorldInfo basicTimeStep lower than normal. An alternate strategy might be to tune the PID control to simulate the physical leg spring.
This model is demonstrated in the boom-monopod.wbt world available within the
Webots.zip archive.
Screenshot of Webots model of the boom-monopod. For scale, the boom tube defaults to 1 meter long.¶
Boom Monopod Proto¶
1#VRML_SIM R2023b utf8
2# Actuated monopod on the end of a passive boom. The boom base has NULL physics
3# so it will be fixed in place.
4# documentation url: https://courses.ideate.cmu.edu/16-375
5# license: No copyright, 2020-2024 Garth Zeglin. This file is explicitly placed in the public domain.
6# template language: javascript
7PROTO boom-monopod [
8 field SFVec3f translation 0 0 0
9 field SFRotation rotation 0 1 0 0
10 field SFFloat boomLength 1.0
11 field SFFloat legLength 0.2
12 field SFString controller "boom_monopod"
13 field SFString name "monopod"
14 field SFString customData ""
15]
16{
17 Robot {
18 # connect properties to user-visible data fields
19 translation IS translation
20 rotation IS rotation
21 controller IS controller
22 name IS name
23 customData IS customData
24
25 # calculate derived parameters
26 %<
27 let boomLength = fields.boomLength.value;
28 let legLength = fields.legLength.value;
29 let baseCylinderHeight = 0.050;
30 let boomYHeight = baseCylinderHeight + 0.050;
31 let halfBoomLen = 0.5*boomLength;
32 let halfLegLength = 0.5*legLength;
33 let boomRise = legLength - boomYHeight + 0.1050;
34 let boomAngle = Math.asin(boomRise/boomLength);
35 >%
36
37 # define the kinematic tree
38 children [
39
40 # add a default radio receiver and transmitter
41 Receiver {
42 channel 1
43 }
44 Emitter {
45 channel 1
46 }
47
48 # cylindrical base shape
49 Pose {
50 translation 0 0 %<= 0.5*baseCylinderHeight >%
51 children [
52 Shape {
53 appearance DEF boomAppearance PBRAppearance {
54 baseColor 0.21529 0.543008 0.99855
55 metalness 0.5
56 roughness 0.5
57 }
58 geometry Cylinder {
59 height %<= baseCylinderHeight >%
60 radius 0.2
61 }
62 }
63 ]
64 }
65 # define the base pivot joint connecting the base
66 # and the first link
67 HingeJoint {
68 jointParameters HingeJointParameters {
69 axis 0 0 1
70 }
71 device [
72 PositionSensor {
73 name "boom_z"
74 }
75 ]
76 # define the body in the center of the base universal joint
77 endPoint Solid {
78 translation 0 0 %<= baseCylinderHeight >%
79 physics Physics { }
80 # define a body to represent the joint center
81 children [
82 DEF blockShape Pose {
83 translation 0 0 0.025
84 children [
85 Shape {
86 appearance USE boomAppearance
87 geometry Box {
88 size 0.1 0.1 0.050
89 }
90 }
91 ]
92 }
93
94 # define the second axis of the base joint
95 HingeJoint {
96 jointParameters HingeJointParameters {
97 axis 0 1 0
98 anchor 0 0 0.050
99 }
100 device [
101 PositionSensor {
102 name "boom_y"
103 }
104 ]
105 # start definition of the boom link, with coordinates rotated so +X is along
106 # the tilted slope of the boom
107 endPoint Solid {
108 translation 0 0 0.050
109 rotation 0 -1 0 %<= boomAngle >%
110 physics Physics { density 300 }
111 children [
112 # boom cylinder shape
113 DEF boomShape Pose {
114 translation %<= halfBoomLen >% 0 0.0
115 rotation 0 1 0 1.5708
116 children [
117 Shape {
118 appearance USE boomAppearance
119 geometry Cylinder {
120 radius 0.025
121 height %<= fields.boomLength.value >%
122 }
123 }
124 ]
125 }
126 # define the boom_x joint at the end of the boom for body pitch rotation
127 # the axis is rotated so the +X is once again level
128 HingeJoint {
129 jointParameters HingeJointParameters {
130 axis %<= Math.cos(boomAngle) >% 0 %<= -Math.sin(boomAngle) >%
131 anchor %<= fields.boomLength.value >% 0 0
132 }
133 device [
134 PositionSensor {
135 name "boom_x"
136 }
137 RotationalMotor {
138 name "pitch"
139 }
140 ]
141 # define the block at the end of the boom representing the body mass
142 endPoint Solid {
143 translation %<= fields.boomLength.value >% 0 0
144 rotation 0 1 0 %<= boomAngle >%
145 physics Physics { density 300 }
146 children [
147 DEF boomEndShape Pose {
148 translation 0.050 0 0
149 children [
150 Shape {
151 appearance USE boomAppearance
152 geometry Box {
153 size 0.100 0.100 0.050
154 }
155 }
156 ]
157 } # end boomEndShape Pose
158
159 # define the 'monopod' vertical leg joint as a slider
160 # positive displacement lengthens the leg
161 SliderJoint {
162 jointParameters JointParameters {
163 axis 0 0 -1
164 minStop -0.2
165 maxStop 0.0
166 }
167 device [
168 PositionSensor {
169 name "leg_encoder"
170 }
171 LinearMotor {
172 name "leg_actuator"
173 maxForce 100
174 }
175 ]
176 # define the monopod leg and foot
177 # the coordinates are rotated so Z is vertical w.r.t world
178 endPoint Solid {
179 translation 0.1 0 0
180 physics Physics { density 200 }
181 children [
182 # vertical leg cylinder
183 Pose {
184 # translation 0 0 %<= -0.25 * legLength >%
185 children [
186 Shape {
187 appearance DEF legAppearance PBRAppearance {
188 baseColor 0.413001 1 0.33489
189 metalness 0.5
190 }
191 geometry Cylinder {
192 height %<= 2.0*legLength >%
193 radius 0.020
194 }
195 } # end leg cylinder Shape
196 ] # end children of leg Pose
197 } # end leg Pose
198 # smaller foot cylinder, turned on side
199 DEF footShape Pose {
200 translation 0 0 %<= -legLength - 0.050 >%
201 rotation 0 1 0 1.5708
202 children [
203 Shape {
204 appearance USE legAppearance
205
206 geometry Cylinder {
207 height 0.030
208 radius 0.050
209 }
210 } # end foot Shape
211 ] # end children of foot Pose
212 } # end foot Pose
213 ] # end children of monopod endPoint Solid
214 boundingObject USE footShape
215 } # end endPoint Solid for monopod
216 } # end monopod leg SliderJoint
217 ] # end children of endPoint Solid for boom end block
218 boundingObject USE boomEndShape
219 } # end endPoint Solid for boom end block
220 } # end boom_x HingeJoint
221 ] # end children of boom_y endpoint Solid
222 boundingObject USE boomShape
223 } # end boom_y endpoint Solid
224 } # end boom_y HingeJoint
225 ] # end children of boom_z endpoint Solid
226 boundingObject USE blockShape
227 } # end boom_z endpoint Solid
228 } # end boom_z HingeJoint
229 ] # end children of Robot
230 } # end Robot
231} # end body of proto
boom_monopod.py¶
1# boom_monopod
2# Copyright, 2024, Garth Zeglin.
3# Sample Webots controller file for driving a monopod mounted on a passive boom.
4
5print("boom_monopod.py waking up.")
6
7# Import standard Python libraries.
8import math
9
10# Import the Webots simulator API.
11from controller import Robot
12
13# Define the time step in milliseconds between controller updates.
14EVENT_LOOP_DT = 5
15
16################################################################
17class Monopod(Robot):
18 def __init__(self):
19
20 super(Monopod, self).__init__()
21 self.robot_name = self.getName()
22 print("%s: controller connected." % (self.robot_name))
23
24 # Connect to the radio receiver.
25 self.receiver = self.getDevice('receiver')
26 self.receiver.enable(100) # milliseconds of sampling period
27 self.message_base_address = "/" + self.robot_name
28
29 # Fetch handles for the motors.
30 self.pitch_motor = self.getDevice('pitch')
31 self.leg_actuator = self.getDevice('leg_actuator')
32
33 # Use the leg linear actuator in force mode.
34 self.leg_actuator.setPosition(math.inf)
35 self.leg_actuator.setForce(0)
36
37 # The controller uses a fixed thrust on takeoff with no altitude control.
38 # The units are meters of leg spring compression.
39 self.thrust = 0.040
40
41 # Target speed in boom_z radians/sec.
42 self.speed = 1.0
43
44 # Fetch handles for the joint sensors.
45 self.boom_z = self.getDevice('boom_z')
46 self.boom_y = self.getDevice('boom_y')
47 self.boom_x = self.getDevice('boom_x')
48 self.leg_len = self.getDevice('leg_encoder')
49
50 # Enable the sensors at full cycle rate.
51 for s in [self.boom_z, self.boom_y, self.boom_x, self.leg_len]:
52 s.enable(EVENT_LOOP_DT)
53
54 # Initialize state estimation.
55 self.delta_t = EVENT_LOOP_DT * 0.001
56 self.sim_time = 0.0
57 self.b_x, self.b_y, self.b_z = (0.0, 0.0, 0.0)
58 self.b_xd, self.b_yd, self.b_zd = (0.0, 0.0, 0.0)
59 self.b_x_last, self.b_y_last, self.b_z_last = (0.0, 0.0, 0.0)
60 self.leg_length = 0.0
61
62 # The pitch motor is driven in position mode.
63 self.pitch_motor.setPosition(0.0)
64
65 #================================================================
66 def poll_sensors(self):
67 """Read sensors and update filters."""
68
69 # Read the boom angles to determine the spatial position. A positive b_z is
70 # forward travel, a positive b_y is foot to ground contact, a positive b_x
71 # is negative pitch, i.e. foot placed forward.
72 self.b_z = self.boom_z.getValue()
73 self.b_y = self.boom_y.getValue()
74 self.b_x = self.boom_x.getValue()
75
76 # Calculate velocities using finite difference.
77 self.b_zd = (self.b_z - self.b_z_last) / self.delta_t
78 self.b_yd = (self.b_y - self.b_y_last) / self.delta_t
79 self.b_xd = (self.b_x - self.b_x_last) / self.delta_t
80
81 self.b_x_last, self.b_y_last, self.b_z_last = (self.b_x, self.b_y, self.b_z)
82
83 # Read the leg length sensor as a step toward simulating a leg spring. Leg
84 # length is positive as the leg extends, i.e., is normally decreasing during
85 # landing and increasing during takeoff.
86 self.leg_length = self.leg_len.getValue()
87 return
88
89 #================================================================
90 def poll_control(self):
91 """Update control model. Note that the leg spring is simulated using a
92 force actuator, so this also computes the passive leg spring physics."""
93
94 # Choose a pitch angle proportional to the speed error.
95 self.pitch_motor.setPosition(-0.3 * (self.speed - self.b_zd))
96
97 # Superimpose thrust force during liftoff phase. The condition
98 # is that the foot is in contact but the boom velocity is upwards.
99 if self.b_y > 0.0 and self.b_yd < 0.0:
100 neutral = self.thrust
101 else:
102 neutral = 0.0
103
104 # Apply simulated spring force. Positive forces extend the leg, i.e., press
105 # the foot against the ground. A typical equilibrium force is about 6N
106 # representing the weight of the boom. The spring constant is chosen k = F / d
107 # for roughly 50 mm of compression at equilibrium.
108 spring_force = -120.0 * (self.leg_length - neutral)
109 self.leg_actuator.setForce(spring_force)
110
111 # print(f"boom: y:{b_y}, z:{b_z} leg_length: {leg_length} neutral: {neutral} force: {spring_force}")
112 return
113
114 #================================================================
115 def poll_receiver(self):
116 """Process all available radio messages."""
117 while self.receiver.getQueueLength() > 0:
118 packet = self.receiver.getString()
119 tokens = packet.split()
120 if len(tokens) >= 2:
121 if tokens[0].startswith(self.message_base_address):
122 if tokens[0] == self.message_base_address + '/speed':
123 self.speed = float(tokens[1])
124 print("%s: set speed to %f" % (robot_name, self.speed))
125 # done with packet processing, advance to the next packet
126 self.receiver.nextPacket()
127
128 #================================================================
129 def run(self):
130 # Run loop to execute a periodic script until the simulation quits.
131 # If the controller returns -1, the simulator is quitting.
132 while self.step(EVENT_LOOP_DT) != -1:
133 # Read simulator clock time.
134 self.sim_time = self.getTime()
135
136 # Process sensor values.
137 self.poll_sensors()
138
139 # Poll the simulated radio receiver.
140 self.poll_receiver()
141
142 # Update control.
143 self.poll_control()
144
145
146################################################################
147# Start the script.
148robot = Monopod()
149robot.run()