Goal

Our goal was to create the movement of a front kick that a human could produce. We started off getting a double pendulum in a Python Template to replicate the general motion of the dynamic movement.

Outcomes

If we could have done it better another time, we would have made the motion more realistic/natural. We attempted to do that with minimal PID Control by adjusting the double pendulum .py file where we adjusted the Dampening . We added keyframes that are specific to our motion but we did not figure out a way to increase the number of keyframes in the loop.

Code
dblpend_keyframes.py
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
#!/usr/bin/env python3
"""Prototype Python 3 script for the 16-375 double-pendulum control exercise."""
 
################################################################
# Written in 2018-2019 by Garth Zeglin <garthz@cmu.edu&gt;
 
# To the extent possible under law, the author has dedicated all copyright
# and related and neighboring rights to this software to the public domain
# worldwide. This software is distributed without any warranty.
 
# You should have received a copy of the CC0 Public Domain Dedication along with this software.
# If not, see <http://creativecommons.org/publicdomain/zero/1.0/&gt;.
 
################################################################
import math
import numpy as np
 
import rcp.doublependulum
from rcp.ex.dblpend import main
 
################################################################
class PendulumController(rcp.doublependulum.DoublePendulumController):
    def __init__(self):
        super().__init__()
 
        # for this example, there are four fixed poses commanded a fixed intervals, expressed as a set of (q0, q1) pairs
        self.keyframes = np.zeros((4,2))
        self.last_frame = None
        return
 
    #================================================================
    def setup(self):
        self.write("""Keyframed position control demonstration.
 
Applies a sequence of joint targets to a position controller at fixed intervals.
 
The parameter sliders 0 and 1 control the 'shoulder' and 'elbow' angles for the first keyframe, 2 and 3 are the second keyframe, etc.
 
""")
        return
 
    #================================================================
    def user_parameter_change(self, parameter, value):
        pose = parameter // 2
        joint = parameter % 2
        if pose <  len(self.keyframes):
            self.keyframes[pose][joint] = 2 * math.pi * (value - 0.5)
            self.write("Set pose %d joint %d to angle %f" % (pose, joint, self.keyframes[pose][joint]))
 
    def apply_configuration(self, config):
        self.keyframes[0,:] = [-1.42,-1.95]
        self.keyframes[1,:] = [2.5,-0.3]
        self.keyframes[2,:] = [0.625,-0.36]
        self.keyframes[3,:] = [-0.1,-0.07]
        return
 
        # if 'keyframes' in config:
        #     for i in range(len(self.keyframes)):
        #         q0 = config['keyframes'].getfloat('keyframe-%d-q0' % i, fallback = 0.0)
        #         q1 = config['keyframes'].getfloat('keyframe-%d-q1' % i, fallback = 0.0)
        #         self.keyframes[i,:] = [q0, q1]
        # return
 
 
 
    def gather_configuration(self, config):
        if 'keyframes' not in config:
            config['keyframes'] = {}
        for i in range(len(self.keyframes)):
            config['keyframes']['keyframe-%d-q0' % i] = str(self.keyframes[i, 0])
            config['keyframes']['keyframe-%d-q1' % i] = str(self.keyframes[i, 1])
        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 numpy array with joint positions ``q`` and joint velocities ``qd`` as ``[q0, q1, qd0, qd1]``, expressed in radians and radians/sec
        :param tau: two element numpy array to fill in with the joint torques to apply ``[tau0, tau1]``
        """
        # select the current keyframe based on the time
        frame  = int(t // 1.5)
        if frame != self.last_frame:
            self.write("Starting frame %d" % frame)
        self.last_frame = frame
 
        # select the pose for the current keyframe, looping over the available poses
        pose = self.keyframes[frame % len(self.keyframes)]
 
        # create a target state by extending the pose to include velocity
        target = np.concatenate((pose, np.zeros(2)))
 
        # calculate position and velocity error as difference from reference state
        qerr = target - state
 
        # 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])
 
        return
 
################################################################$
if _name_ == "_main_":
    main(PendulumController)
doublependulum.py
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
# Standard library modules.
import math, time, threading, queue, logging
 
# Third-party library modules.
import numpy as np
 
# set up logger for module
log = logging.getLogger(_file_)
 
################################################################
class DoublePendulumController(object):
    """Prototype for a double-pendulum controller.  This class is typically subclassed to customize the control strategy and the subclass passed into the common application framework.
 
    :ivar initial_state: four-element numpy array used to initialize the simulator state: [q1, q2, qd1, qd2]
    :ivar kp: two-element numpy array with default proportional position gains
    :ivar ki: two-element numpy array with default integral gains
    :ivar kd: two-element numpy array with default velocity damping gain
    """
    def __init__(self):
 
        # delegate object to which debugging and status messages can be printed; it must implement write()
        self.console = None
 
        # fixed controller parameters
        self.initial_state = np.array([0.0, 0.0, 0.0, 0.0])
        self.d_state       = np.array((1.0, 0.5*math.pi, 0.0, 0.0))
        self.kp      = np.array((16.0, 8.0))
        self.ki      = np.array((4.0, 2.0))
        self.kd      = np.array((10.0, 2.0))
         
        return
 
    #================================================================
    def connect_console(self, console):
        """Attach a console object to be used for debugging output.  The object needs to implement write()."""
        self.console = console
        return
     
    def write(self, string):
        """Write a message to the debugging console.  If console is not available, writes to the log as an info message."""
        if self.console is not None:
            self.console.write(string)
        else:
            log.info(string)
        return
     
    def setup(self):
        """Hook for final one-time object configuration.  This is called once prior to the start of the simulation.  The default implementation does nothing."""
        pass
 
    def user_parameter_change(self, parameter, value):
        """Hook for interactive parameter changes (e.g. GUI sliders).  The default implementation does nothing.
 
        :param parameter: integer index of parameter, starting with zero
        :param value: dimensionless parameter value, ranges from zero to one inclusive.
        """
        pass
    #================================================================
    def apply_configuration(self, config):
        """Hook for applying parameters saved in a configuration file. The default implementation does nothing.
 
        :param config: configparser object (which implements the mapping protocol)
        """
        pass
 
    def gather_configuration(self, config):
        """Hook for saving parameters in a configuration file. The default implementation does nothing.
 
        :param config: configparser object (which implements the mapping protocol)
        """
        pass
     
    #================================================================
    def compute_control(self, t, dt, state, tau):
        """Method called from numerical pendulum simulation to calculate the next step of applied torques.  This is usually overridden in a subclass, but the default implementation applies a fixed-target PD position control.
 
        :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
 
        # calculate position and velocity error as difference from reference state
        qerr = self.d_state - state
 
        # 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])
 
        return
 
################################################################
class DoublePendulumSimulator(object):
    """Numerical dynamic simulation of a frictionless double pendulum.  It
    communicates with a user-supplied control object to compute applied joint
    torques.
    """
 
    def __init__(self):
        # the object used to calculate joint torques
        self.control = None
 
        # set default dynamics
        self.set_default_dynamic_parameters()
 
        # configure transient state
        self.reset()
        return
 
    def connect_controller(self, controller):
        """Attach a controller object used to compute joint torques and set the initial state."""
        self.control = controller
        self.reset()
        return
     
    def reset(self):
        """Reset or initialize all simulator state variables."""
        self.t     = 0.0
        self.dt    = 0.001
        if self.control is not None:
            self.state = self.control.initial_state.copy()
        else:
            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
 
################################################################