Group: Xin Hui Lim, Soonho Kwon, and Wade Lacey

Goals:

We wanted to simulate our double pendulum to go through four predetermined poses.  For pose one the pedulum would move up to a bent pose and hold for five seconds before proceding.  Then to make this cycle of movements more interesting we decided the simulation should move back and forth between the second and third pose an undefined number of times until it reached a specific position when then it could proceed onto the next pose, pose four.  After the simulation reached pose four we then wanted it to fall back down to neutral.  When the double pendulum goes back down to the neutral point the whole process will restart. To incorporate the optimization, we decide to apply optimization techniques to the number of times the pendulum swings between pose two and three before reaching the position to trigger pose four.

Outcomes:

We got the simulation to progress through the poses as we had planned. The movement this created is acrobatic in a sense.  To us it looks like a gymnast gaining momentum and propelling themself up into a vertical handstand above the bar.  Then returning to neutral by swining back down.  Next time learning to get better and better or more efficient at propelling itself up into  and through the vertical position.

Final Code:

Defining the 4 poses or targets we wanted the double pendulum to move through.

The code that gives the simulation the torques to move it to the poses we wanted in the order and manner desired.  Use of PID and PD controllers to find the torques needed to get to the poses correctly.  Optimization using cost function for the number of swings between pose 2 and 3 before moving on to pose 4.

Original plan sketch:

 

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
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
#Soonho Kwon, Xin Hui Lim, Wade Lacey
 
#!/usr/bin/env python3
 
# Enable basic compatibility features to work with either Python 2 or 3.
from __future__ import print_function, absolute_import, unicode_literals
 
# Standard library modules.
import math, time, argparse, threading, queue
 
# Third-party library modules.
import numpy as np
import time
 
# Use the scipy fmin optimizer if available.
try:
    import scipy.optimize
    have_optimizer = True
except ImportError:
    print("Unable to load scipy.optimize.")
    have_optimizer = False
 
# This uses python-osc to communicate with a Max/MSP patch.
#   installation:      pip3 install python-osc
#   source code:       https://github.com/attwad/python-osc
#   pypi description:  https://pypi.org/project/python-osc/
from pythonosc import udp_client
from pythonosc import dispatcher
from pythonosc import osc_server
 
################################################################
class DoublePendulumController(object):
    """Prototype for a double-pendulum controller.  This is where you should
    customize the underlying control functions, optimization strategy, and
    reward functions.
 
    :param args: namespace of command-line arguments returned from argparse
    """
    def __init__(self, args):
 
        # initialize mode and state variables for controller
        self.mode = 'dissipate'
        self.cycle_start = 0.0
        self.cycle_cost = 0.0
 
        # fixed controller parameters
        self.friction_damping = -0.2
 
        self.hand_up_target = np.array((-0.25*math.pi,.75*math.pi,0,0))
 
        self.hand_up_target2 = np.array((-0.25*math.pi, 0.5*math.pi, 6, 0))
 
        self.hand_up_target3 = np.array((0.25*math.pi, 0.5*math.pi, -6, 0))
 
        self.hand_up_target4 = np.array((-math.pi, 0, 0, 0))
 
        self.kp    = np.array((16.0, 8.0))
        self.ki    = np.array((4.0, 2.0))
        self.kd    = np.array((4.0, 2.0))
 
        # integrated error
        self.ierr  = np.array((0.0, 0.0, 0.0, 0.0))
 
        # keep track of the worst case cost to know when to cancel a trial
        self.worst_cost = None
 
        # Construct default parameter vector for the optimization problem.  This
        # builds a single array with all the default parameter values.  See the
        # set_optimization_params() method to update the mapping from parameter
        # vector back to controller parameters.
        self.default_params = np.hstack((self.kp, self.ki, self.kd))
 
        # launch a thread to run the optimizer as a co-routine
        self.evaluation_queue = queue.Queue() # next parameter array to evaluate
        self.result_queue     = queue.Queue() # next computed cost value
        self.optimizer_thread = threading.Thread(target=self.optimizer_task)
        self.optimizer_thread.daemon = True
        self.optimizer_thread.start()
        return
 
    #================================================================
 
    def message(self, msgaddr, *args):
        """Process messages from the Max/MSP display received via OSC over UDP.
 
        :param msgaddr: the address string of the OSC message, eg '/control/mode'
        :param args: tuple of OSC message arguments
        """
 
        print("Controller received message %s: %s" % (msgaddr, args))
        if msgaddr == "/control/kp1":
            self.kp[0] = args[0]
        elif msgaddr == "/control/kd1":
            self.kd[0] = args[0]
        elif msgaddr == "/control/mode":
            self.mode = args[0]
 
    #================================================================
    def optimizer_task(self):
        """Entry point for starting the optimizer on a background thread.  It communicates with the simulator using a pair of queues."""
        print("Starting optimizer thread.")
        if have_optimizer:
            result = scipy.optimize.fmin(func=self.optimizer_eval, x0=self.default_params)
            print("Optimizer finished:", result)
        else:
            print("Optimizer not available, evaluating default parameters.")
            result = self.optimizer_eval(self.default_params)
        return
 
    def optimizer_eval(self, params):
        """Optimizer callback function to evaluate a parameter vector.  This pushes the
        vector into a queue and blocks the optimizer thread waiting for a cost result.
 
        :param params: numpy ndarray for the optimization problem parameters
        :returns: a scalar cost value
 
        """
 
        print("Optimizer requesting evaluation of:", params)
        self.evaluation_queue.put(params)
 
        # wait for result
        cost = self.result_queue.get()
        print("Trial yielded cost:", cost)
        return cost
 
    def set_optimization_params(self, params):
        """Configure controller gains for a single trial of the optimization problem.
        Called from within the controller state machine when a new parameter
        vector is received for evaluation.
 
        :param params: numpy ndarray of optimization parameters
        """
        self.kp = params[0:2]
        self.ki = params[2:4]
        self.kd = params[4:6]
        print("Set optimization params: kp is %s, ki is %s, kd is %s" % (self.kp, self.ki, self.kd))
        return
 
    #================================================================
    def compute_control(self, t, dt, state, tau):
        """Method called from simulator to calculate the next step of applied torques.
 
        :param t: time in seconds since simulation began
        :param state: four element ndarray of joint positions q and joint velocities qd as [q1, q2, qd1, qd2], expressed in radians and radians/sec
        :param tau: two element ndarray to fill in with joint torques to apply
        """
        # convenience variables to notate the state variables
        q1  = state[0# 'shoulder' angle in radians
        q2  = state[1# 'elbow' angle in radians
        qd1 = state[2# 'shoulder' velocity in radians/second
        qd2 = state[3# 'elbow' velocity in radians/second
 
        # print(q1)
 
        if self.mode == 'pose':
 
            if t<4:
                qerr = self.hand_up_target - state #first target diff
 
                tau[0] = (self.kp[0] * qerr[0]) + (self.kd[0] * qerr[2]) + (self.ki[0] * self.ierr[0]) #gets to first target
                tau[1] = (self.kp[1] * qerr[1]) + (self.kd[1] * qerr[3]) + (self.ki[1] * self.ierr[1])
                self.ierr = self.ierr + qerr * dt
 
            else:
                if not self.evaluation_queue.empty():
                    params = self.evaluation_queue.get()
                    self.set_optimization_params(params)
                    self.cycle_cost = 0.0
 
                if qd1 < 0:
                    qerr = self.hand_up_target3 - state
                    #print("target3",qd1)
 
                elif qd1 > 0:
                    qerr = self.hand_up_target2 - state
                    #print("target2",qd1)
 
                # apply PID control to reach the pose
                tau[0] = (self.kp[0] * qerr[0]) + (self.kd[0] * qerr[2]) + (self.ki[0] * self.ierr[0])
                tau[1] = (self.kp[1] * qerr[1]) + (self.kd[1] * qerr[3]) + (self.ki[1] * self.ierr[1])
                self.cycle_cost+=1
 
                # integrate the error
                self.ierr = self.ierr + qerr * dt
 
                if abs(qd1)>3:
                    self.mode='pose4'
                    print("pose4")
 
        # select action based on mode
 
        elif self.mode == 'pose4':
            qerr = self.hand_up_target4 - state
            #print("target4",qd1)
 
            # apply PD control to reach the pose (no integral term)
            tau[0] = (self.kp[0] * qerr[0]) + (self.kd[0] * qerr[2])
            tau[1] = (self.kp[1] * qerr[1]) + (self.kd[1] * qerr[3])
            self.cycle_cost+=1
 
            if abs(qerr[0])<0.1:
                self.mode='home'
                print('home')
 
        elif self.mode == 'home':
            qerr = self.hand_up_target - state
            tau[0] = (self.kp[0] * qerr[0]) + (self.kd[0] * qerr[2])
            tau[1] = (self.kp[1] * qerr[1]) + (self.kd[1] * qerr[3])
            self.cycle_cost+=1
 
            if abs(qerr[0])<0.1 and abs(qd1)<0.01:
                tau[0] = 100*self.friction_damping * qd1
                tau[1] = 100*self.friction_damping * qd2
                # calculate position and velocity error as difference from reference state
                self.ierr  = np.array((0.0, 0.0, 0.0, 0.0)) # reset integrator
                self.result_queue.put(self.cycle_cost)
                if self.worst_cost is None: self.worst_cost = self.cycle_cost
                else: self.worst_cost = max(self.worst_cost, self.cycle_cost)
 
                if self.worst_cost is not None and self.cycle_cost > 2*self.worst_cost:
                    print("Cancelling trial at cost: ", self.cycle_cost)
                    self.mode = 'hand-down'
                    self.ierr  = np.array((0.0, 0.0, 0.0, 0.0)) # reset integrator
                    self.result_queue.put(self.cycle_cost)
 
                print("end of cycle")
                self.mode = 'pose'
 
        else: # free-swinging, no torques
            tau[0] = 0.0
            tau[1] = 0.0
 
        # accumulate sum of squared torques cost
        #self.torque_cost += tau.dot(tau)
        return
 
################################################################
class DoublePendulumSimulator(object):
    """Dynamic simulation of a frictionless double pendulum.  This object can send
    state updates to an external server for rendering.  It communicates with a
    user-supplied control object to compute applied joint torques.
 
    :param args: namespace of command-line arguments returned from argparse
    :param cartoon: python-osc UDP client object for sending messages to GUI
    :param control: instance of DoublePendulumControl for computing applied torques
    """
 
    def __init__(self, args, cartoon, control):
        # save the OSC client object used to update the display
        self.cartoon = cartoon
 
        # save the object used to calculate joint torques
        self.control = control
 
        # save selected command line flags
        self.verbose = args.verbose
        self.fast = args.fast
 
        # set default dynamics
        self.set_default_dynamic_parameters()
 
        # configure transient state
        self.reset()
        return
 
    def reset(self):
        """Reset or initialize all simulator state variables."""
        self.t     = 0.0
        self.dt    = 0.001
        self.state = np.array([0.0, 0.0, 0.0, 0.0])
        self.tau   = np.array([0.0, 0.0])
        self.dydt  = np.ndarray((4,))
        return
 
    def set_default_dynamic_parameters(self):
        """Set the default dynamics coefficients defining the rigid-body model physics."""
        self.l1   = 1.0    # proximal link length, link1
        self.l2   = 1.0    # distal link length, link2
        self.lc1  = 0.5    # distance from proximal joint to link1 COM
        self.lc2  = 0.5    # distance from distal joint to link2 COM
        self.m1   = 1.0    # link1 mass
        self.m2   = 1.0    # link2 mass
        self.I1   = (self.m1 * self.l1**2) / 12  # link1 moment of inertia
        self.I2   = (self.m2 * self.l2**2) / 12  # link2 moment of inertia
        self.gravity  = -9.81
        return
 
    #================================================================
    def deriv(self):
        """Calculate the accelerations for a rigid body double-pendulum dynamics model.
        :returns: system derivative vector as a numpy ndarray
        """
        q1  = self.state[0]
        q2  = self.state[1]
        qd1 = self.state[2]
        qd2 = self.state[3]
        LC1 = self.lc1
        LC2 = self.lc2
        L1 = self.l1
        M1 = self.m1
        M2 = self.m2
 
        d11 = M1*LC1*LC1  + M2*(L1*L1 + LC2*LC2 + 2*L1*LC2*math.cos(q2)) + self.I1 + self.I2
        d12 = M2*(LC2*LC2 + L1*LC2*math.cos(q2)) + self.I2
        d21 = d12
        d22 = M2*LC2*LC2  + self.I2
 
        h1 = -M2*L1*LC2*math.sin(q2)*qd2*qd2 - 2*M2*L1*LC2*math.sin(q2)*qd2*qd1
        h2 = M2*L1*LC2*math.sin(q2)*qd1*qd1
 
        phi1 = -M2*LC2*self.gravity*math.sin(q1+q2)  - (M1*LC1 + M2*L1) * self.gravity * math.sin(q1)
        phi2 = -M2*LC2*self.gravity*math.sin(q1+q2)
 
        # now solve the equations for qdd:
        #  d11 qdd1 + d12 qdd2 + h1 + phi1 = tau1
        #  d21 qdd1 + d22 qdd2 + h2 + phi2 = tau2
 
        rhs1 = self.tau[0] - h1 - phi1
        rhs2 = self.tau[1] - h2 - phi2
 
        # Apply Cramer's Rule to compute the accelerations using
        # determinants by solving D qdd = rhs.  First compute the
        # denominator as the determinant of D:
        denom = (d11 * d22) - (d21 * d12)
 
        # the derivative of the position is trivially the current velocity
        self.dydt[0] = qd1
        self.dydt[1] = qd2
 
        # the derivative of the velocity is the acceleration.
        # the numerator of qdd[n] is the determinant of the matrix in
        # which the nth column of D is replaced by RHS
        self.dydt[2] = ((rhs1 * d22 ) - (rhs2 * d12)) / denom
        self.dydt[3] = (( d11 * rhs2) - (d21  * rhs1)) / denom
        return self.dydt
 
    #================================================================
    def timer_tick(self, delta_t):
        """Run the simulation for an interval.
 
        :param delta_t: length of interval in simulated time seconds
        """
        while delta_t > 0:
 
            # calculate next control outputs
            self.control.compute_control(self.t, self.dt, self.state, self.tau)
 
            # calculate dynamics model
            qd = self.deriv()
 
            # Euler integration
            self.state = self.state + self.dt * qd
            delta_t -= self.dt
            self.t += self.dt
 
    def update_display(self):
        """Send the current state to the GUI using a UDP OSC message."""
        radian_to_degrees = 180.0 / math.pi
        pose = (self.state[0]*radian_to_degrees, self.state[1]*radian_to_degrees)
        self.cartoon.send_message("/q", pose)
        if self.verbose: print("Position: ", pose)
        return
 
    def event_loop(self):
        """Simulator run loop; never exits."""
        frame_interval = 0.040 if not self.fast else 10.0
        while True:
            self.timer_tick(frame_interval)
            self.update_display()
            if not self.fast: time.sleep(frame_interval)
 
    #================================================================
    # Methods to process messages from the Max/MSP display received via OSC over UDP.
    def message(self, msgaddr, *args):
        """Process messages from the Max/MSP display received via OSC over UDP.
 
        :param msgaddr: the address string of the OSC message, eg '/sim/verbose'
        :param args: tuple of OSC message arguments
        """
 
        print("Simulator received message %s: %s" % (msgaddr, args))
        if msgaddr == "/sim/verbose":
            self.verbose = (args[0] != 0)
 
    def unknown_message(self, msgaddr, *args):
        """Default handler for unrecognized OSC messages."""
        print("Simulator received unmapped message %s: %s" % (msgaddr, args))
 
################################################################
def start_osc_server(args, sim, control):
    """Start a background thread running an OSC server listening for messages on an UDP socket."""
 
    # Initialize the OSC message dispatch system.
    dispatch = dispatcher.Dispatcher()
    dispatch.map("/sim/*", sim.message)
    dispatch.map("/control/*", control.message)
    dispatch.set_default_handler(sim.unknown_message)
 
    # Start and run the server.
    server = osc_server.ThreadingOSCUDPServer((args.simaddr, args.simport), dispatch)
    server_thread = threading.Thread(target=server.serve_forever)
    server_thread.daemon = True
    server_thread.start()
 
################################################################
# Script entry point.
if __name__ == '__main__':
    parser = argparse.ArgumentParser(description="Double-pendulum or two-link arm simulator.")
    parser.add_argument('--verbose', action='store_true', help='Enable debugging output.')
    parser.add_argument('--maxport', type=int, default=16375, help='UDP port for Max/MSP patch (default 16375).')
    parser.add_argument('--maxaddr', default="127.0.0.1", help='IP address for the Max/MSP patch (default localhost).')
    parser.add_argument('--simport', type=int, default=54375, help='UDP port for Python simulator (default 54375).')
    parser.add_argument('--simaddr', default="127.0.0.1", help='IP address for the simulator (default localhost).')
    parser.add_argument('--fast', action='store_true', help='Run as fast as possible.')
    args = parser.parse_args()
 
    # create an OSC client endpoint to send display updates to Max/MSP
    cartoon = udp_client.SimpleUDPClient(args.maxaddr, args.maxport)
 
    # initializes the simulator
    control = DoublePendulumController(args)
    sim = DoublePendulumSimulator(args, cartoon, control)
 
    # set up the OSC message server to receive parameters from Max/MSP
    start_osc_server(args, sim, control)
 
    # begin the simulation
    sim.event_loop()