<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 Quadruped Gait
Kevin Walchko, created 8 Nov 2016
Step3:  Hildebrand Diagram
Example diagram from wikipedia
Step5:  Discrete Ripple Gait
The gait uses an array to hold the leg/foot positions in an attempt to simplify the coding and reduce the computational requirements.
Step9:  Continous Ripple Gait
Unlike the discrete gait above, this continous gait can be interpolated into finer steps in an attempt to slow down the speed of the gait or produce smoother movements.
Step10:  Gait Plot
Now select which gait you want and run it for just one leg. In reality you would need to do this for all 4 legs.
Step11:  Now the code below runs the for all 4 legs and only prints out the position for leg 0. You can modify this above in the printFoot function. In reality, you would pass a function to move the leg.
<ASSISTANT_TASK:>
Python Code:
%matplotlib inline
from __future__ import print_function
from __future__ import division
import matplotlib.pyplot as plt
import numpy as np
from numpy.linalg import norm 
from math import sin, cos, pi, sqrt
from math import radians as d2r
Explanation: Quadruped Gait
Kevin Walchko, created 8 Nov 2016
End of explanation
# def printFoot(i, newpos):
#     if i == 0:
#         print('New [{}](x,y,z): {:.2f}\t{:.2f}\t{:.2f}'.format(i, newpos[0], newpos[1], newpos[2]))
def rot_z(t, c):
    
    t - theta [radians]
    c - [x,y,z] or [x,y] ... the function detects 2D or 3D vector
    
    if len(c) == 3:
        ans = np.array([
            c[0]*cos(t)-c[1]*sin(t),
            c[0]*sin(t)+c[1]*cos(t),
            c[2]
        ])
    else:
        ans = np.array([
            c[0]*cos(t)-c[1]*sin(t),
            c[0]*sin(t)+c[1]*cos(t)
        ])
    return ans
print('{:.2f} {:.2f} {:.2f}'.format(*rot_z(pi/4, np.array([84,0,-65]))))
class Gait(object):
    def __init__(self):
        self.legOffset = [0, 6, 3, 9]
#         self.body = np.array([72.12, 0, 0])
        self.rest = None
        
    def calcRotatedOffset(self, cmd, frame_angle):
        
        calculate the foot offsets for each leg and delta linear/rotational
        in - cmd(x,y,z_rotation)
        out - array(leg0, leg1, ...)
            where leg0 = {'linear': [x,y], 'rotational': [x,y], 'angle': zrotation(rads)}
        
        # I could do the same here as I do below for rotation
        # rotate the command into the leg frame
        rc = rot_z(frame_angle, cmd)
        # get rotation distance: dist = rot_z(angle, rest) - rest
        # this just reduces the function calls and math
        zrot = d2r(float(cmd[2]))  # should I assume this is always radians? save conversion
#         fromcenter = self.rest + self.body
        # value of this?
#         rot = rot_z(zrot/2, fromcenter) - rot_z(-zrot/2, fromcenter)
#         ans = {'linear': rc, 'rotational': rot, 'angle': zrot}
        ans = {'linear': rc, 'angle': zrot}
        return ans
    
    def command(self, cmd, func, steps=12):
        # handle no movement command ... do else where?
        if sqrt(cmd[0]**2 + cmd[1]**2 + cmd[2]**2) < 0.001:
            for leg in range(0, 4):
                func(leg, self.rest)  # move to resting position
            return
        cmd = [100.0, 0.0, 0.0]
        
        # frame rotations for each leg
        frame = [-pi/4, pi/4, 3*pi/4, -3*pi/4]
        for i in range(0, steps):  # iteration, there are 12 steps in gait cycle
            for legNum in [0, 2, 1, 3]:  # order them diagonally
                rcmd = self.calcRotatedOffset(cmd, frame[legNum])
                pos = self.eachLeg(i, rcmd)  # move each leg appropriately
                func(legNum, pos)
Explanation: Hildebrand Diagram
Example diagram from wikipedia:
$\phi$ is the phase of each leg and $z$ is the foot height. The black bars indicate when a foot is in contact with the ground.
Common gaits for quadrupeds (spider/crab configuration):
Ripple Gait: when one leg is in the air while the other three are holding up the body. This tends to be a stable gait when you keep the center of mass (CM) inside the triangle formed by the three legs in contact with the ground. Becuase of the stability, you could interrupt this gait at any time (i.e., stop it) and the robot will not fall over.
Trott Gait: A fast gait where two legs are in the air moving while the other two legs are in contact with the ground. This is an unstable gait and the robot could fall over if the gait is not executed quickly enough.
Terminology:
Duty Factor: Duty factor is simply the percent of the total cycle which a given foot is on the ground. Duty factors over 50% are considered a "walk", while those less than 50% are considered a run. 
Forelimb-hindlimb Phase (or just Phase): is the temporal relationship between the limb pairs.
End of explanation
class DiscreteRippleGait(Gait):
    def __init__(self, h, r):
        Gait.__init__(self)
        self.phi = [9/9, 6/9, 3/9, 0/9, 1/9, 2/9, 3/9, 4/9, 5/9, 6/9, 7/9, 8/9]  # foot pos in gait sequence
        maxl = h  #
        minl = maxl/2
        self.z = [minl, maxl, maxl, minl, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  # leg height
        self.rest = r # idle leg position
    def eachLeg(self, index, cmd):
        
        interpolates the foot position of each leg
        cmd:
            linear (mm)
            angle (rads)
        
        rest = self.rest
        i = index % 12
        phi = self.phi[i]
        # rotational commands -----------------------------------------------
        angle = cmd['angle']/2-cmd['angle']*phi
        rest_rot = rot_z(-angle, rest)
        # linear commands ----------------------------------------------------
        linear = cmd['linear']
        xx = linear[0]
        yy = linear[1]
        # create new move command
        move = np.array([
            xx/2 - phi*xx,
            yy/2 - phi*yy,
            self.z[i]
        ])
        # new foot position: newpos = rest + move ----------------------------
        newpos = move + rest_rot
        return newpos
Explanation: Discrete Ripple Gait
The gait uses an array to hold the leg/foot positions in an attempt to simplify the coding and reduce the computational requirements.
End of explanation
# class ContinousRippleGait(Gait):
#     alpha = 0.5
    
#     def __init__(self, h, r):
#         Gait.__init__(self)
#         self.height = h
#         self.rest = r
    
#     def phi(self, x):
#         
#         The phase 
#         
#         phi = 0.0
#         if x <= 3.0:
#             phi = 1/3*(3.0-x)
#         else:
#             phi = 1/9*(x-3)
#         return phi
#     def z(self, x):
#         
#         Leg height
#         duty cycle:
#             0-3: leg lifted
#             3-12: leg on ground
#             duty = (12-3)/12 = 0.75 = 75% a walking gait
#         
#         height = self.height
#         z = 0.0
#         if x <= 1:
#             z = height/1.0*x
#         elif x <= 2.0:
#             z = height
#         elif x <= 3.0:
#             z = -height/1.0*(x-2.0)+height
#         return z
#     def eachLeg(self, index, cmd):
#         
#         interpolates the foot position of each leg
#         
#         rest = self.rest
#         i = (index*self.alpha) % 12
#         phi = self.phi(i)
#         z = self.z(i)
#         # rotational commands -----------------------------------------------
#         angle = cmd['angle']/2-cmd['angle']*phi
#         rest_rot = rot_z(-angle, rest)
#         # linear commands ----------------------------------------------------
#         linear = cmd['linear']
#         xx = linear[0]
#         yy = linear[1]
#         # create new move command
#         move = np.array([
#             xx/2 - phi*xx,
#             yy/2 - phi*yy,
#             z
#         ])
#         # new foot position: newpos = rest + move ----------------------------
#         newpos = move + rest_rot
#         return newpos
Explanation: Continous Ripple Gait
Unlike the discrete gait above, this continous gait can be interpolated into finer steps in an attempt to slow down the speed of the gait or produce smoother movements.
End of explanation
cmd = {'linear': [0,0], 'angle': pi/4}
leg = np.array([84,0.0,-65]) # idle leg position
height = 25
# gait = ContinousRippleGait(height, leg)
gait = DiscreteRippleGait(height, leg)
alpha = 1
pos = []
for i in range(0,12):
    p = gait.eachLeg(i*alpha,cmd)
    pos.append(p)
    print(p)
Explanation: Gait Plot
Now select which gait you want and run it for just one leg. In reality you would need to do this for all 4 legs.
End of explanation
# Run the entire class
# remember!! command does a rotation of the leg coord system, so it
# will output different numbers than above.
cmd = [0,0,pi/4]
leg = np.array([84,0.0,-65]) # idle leg position
height = 25
# gait = ContinousRippleGait(height, leg)
# gait.alpha = 0.5
gait = DiscreteRippleGait(height, leg)
gait.command(cmd, print, steps=12)  # doesn't return anything
px = []
py = []
pz = []
for p in pos:
    px.append(p[0])
    py.append(p[1])
    pz.append(p[2])
plt.subplot(2,2,1);
plt.plot(px)
plt.ylabel('x')
plt.subplot(2,2,2)
plt.plot(py)
plt.ylabel('y')
plt.subplot(2,2,3);
plt.plot(pz)
plt.ylabel('z')
plt.subplot(2,2,4);
plt.plot(px,py)
plt.ylabel('y')
plt.xlabel('x');
for p in pos:
    print(p)
Explanation: Now the code below runs the for all 4 legs and only prints out the position for leg 0. You can modify this above in the printFoot function. In reality, you would pass a function to move the leg.
End of explanation
<END_TASK>